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: fixed some missing links/poses. GraphViewer: fixed global path color context menu action, now small links are ignored

Browse Source
This commit is contained in:
matlabbe
2015-10-26 17:52:02 -04:00
parent 02ebb3c7e8
commit 8fa7b73aea
2 changed files with 85 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
guilib/src/DatabaseViewer.cpp
View File

@@ -1089,7 +1089,7 @@ void DatabaseViewer::updateIds()
previousPose=p;
//links
bool linksInserted = false;
bool addPose = false;
for(std::map<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
{
std::map<int, Link>::iterator invertedLinkIter = graph::findLink(links, jter->second.to(), jter->second.from(), false);
@@ -1110,11 +1110,14 @@ void DatabaseViewer::updateIds()
{
links_.insert(std::make_pair(ids_[i], jter->second));
}
linksInserted = true;
addPose = true;
}
else if(graph::findLink(links_, jter->second.from(), jter->second.to()) != links_.end())
{
addPose = true;
}
}
if(linksInserted)
if(addPose)
{
poses_.insert(std::make_pair(ids_[i], p));
}

138
guilib/src/GraphViewer.cpp
View File

@@ -301,9 +301,10 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
const Transform & poseA = jterA->second;
const Transform & poseB = jterB->second;
QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.end();
if(_linkItems.contains(idFrom))
{
QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.find(iter->first);
itemIter = _linkItems.find(iter->first);
while(itemIter.key() == idFrom && itemIter != _linkItems.end())
{
if(itemIter.value()->to() == idTo)
@@ -323,79 +324,92 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
interSessionClosure = mapIds.at(jterA->first) != mapIds.at(jterB->first);
}
if(linkItem == 0)
if(poseA.getDistance(poseB) > 0.005)
{
//create a link item
linkItem = new LinkItem(idFrom, idTo, poseA, poseB, iter->second.type(), interSessionClosure);
QPen p = linkItem->pen();
p.setWidthF(_linkWidth);
linkItem->setPen(p);
linkItem->setZValue(10);
this->scene()->addItem(linkItem);
linkItem->setParentItem(_root);
_linkItems.insert(idFrom, linkItem);
if(linkItem == 0)
{
//create a link item
linkItem = new LinkItem(idFrom, idTo, poseA, poseB, iter->second.type(), interSessionClosure);
QPen p = linkItem->pen();
p.setWidthF(_linkWidth);
linkItem->setPen(p);
linkItem->setZValue(10);
this->scene()->addItem(linkItem);
linkItem->setParentItem(_root);
_linkItems.insert(idFrom, linkItem);
}
}
else if(linkItem && itemIter != _linkItems.end())
{
// erase small links
_linkItems.erase(itemIter);
delete linkItem;
linkItem = 0;
}
//update color
if(iter->second.type() == Link::kNeighbor)
if(linkItem)
{
linkItem->setColor(_neighborColor);
}
else if(iter->second.type() == Link::kVirtualClosure)
{
linkItem->setColor(_loopClosureVirtualColor);
}
else if(iter->second.type() == Link::kNeighborMerged)
{
linkItem->setColor(_neighborMergedColor);
}
else if(iter->second.type() == Link::kUserClosure)
{
linkItem->setColor(_loopClosureUserColor);
}
else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure)
{
if(_intraInterSessionColors)
//update color
if(iter->second.type() == Link::kNeighbor)
{
linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor);
linkItem->setZValue(interSessionClosure?8:9);
linkItem->setColor(_neighborColor);
}
else
else if(iter->second.type() == Link::kVirtualClosure)
{
linkItem->setColor(_loopClosureLocalColor);
linkItem->setColor(_loopClosureVirtualColor);
}
}
else
{
if(_intraInterSessionColors)
else if(iter->second.type() == Link::kNeighborMerged)
{
linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor);
linkItem->setZValue(interSessionClosure?8:9);
linkItem->setColor(_neighborMergedColor);
}
else
else if(iter->second.type() == Link::kUserClosure)
{
linkItem->setColor(_loopClosureColor);
linkItem->setColor(_loopClosureUserColor);
}
}
//rejected loop closures
if(_loopClosureOutlierThr > 0.0f)
{
Transform t = poseA.inverse()*poseB;
if(iter->second.to() != idTo)
else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure)
{
t = t.inverse();
}
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
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)
if(_intraInterSessionColors)
{
linkItem->setColor(_loopClosureRejectedColor);
linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor);
linkItem->setZValue(interSessionClosure?8:9);
}
else
{
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 &&
iter->second.type() != Link::kNeighborMerged)
{
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)
{
linkItem->setColor(_loopClosureRejectedColor);
}
}
}
}
@@ -1272,6 +1286,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
this->setLocalPathColor(color);
}
else if(r == aChangeGlobalPathColor)
{
this->setGlobalPathColor(color);
}
else if(r == aChangeIntraSessionLoopColor)
{
this->setIntraSessionLoopColor(color);