i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30: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; previousPose=p;
//links //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) 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); 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)); links_.insert(std::make_pair(ids_[i], jter->second));
} }
addPose = true;
linksInserted = 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)); 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 & poseA = jterA->second;
const Transform & poseB = jterB->second; const Transform & poseB = jterB->second;
QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.end();
if(_linkItems.contains(idFrom)) 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()) while(itemIter.key() == idFrom && itemIter != _linkItems.end())
{ {
if(itemIter.value()->to() == idTo) 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); interSessionClosure = mapIds.at(jterA->first) != mapIds.at(jterB->first);
} }
if(linkItem == 0) if(poseA.getDistance(poseB) > 0.005)
{ {
//create a link item if(linkItem == 0)
linkItem = new LinkItem(idFrom, idTo, poseA, poseB, iter->second.type(), interSessionClosure); {
QPen p = linkItem->pen(); //create a link item
p.setWidthF(_linkWidth); linkItem = new LinkItem(idFrom, idTo, poseA, poseB, iter->second.type(), interSessionClosure);
linkItem->setPen(p); QPen p = linkItem->pen();
linkItem->setZValue(10); p.setWidthF(_linkWidth);
this->scene()->addItem(linkItem); linkItem->setPen(p);
linkItem->setParentItem(_root); linkItem->setZValue(10);
_linkItems.insert(idFrom, linkItem); 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(linkItem)
if(iter->second.type() == Link::kNeighbor)
{ {
linkItem->setColor(_neighborColor); //update color
} if(iter->second.type() == Link::kNeighbor)
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)
{ {
linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor); linkItem->setColor(_neighborColor);
linkItem->setZValue(interSessionClosure?8:9);
} }
else else if(iter->second.type() == Link::kVirtualClosure)
{ {
linkItem->setColor(_loopClosureLocalColor); linkItem->setColor(_loopClosureVirtualColor);
} }
} else if(iter->second.type() == Link::kNeighborMerged)
else
{
if(_intraInterSessionColors)
{ {
linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor); linkItem->setColor(_neighborMergedColor);
linkItem->setZValue(interSessionClosure?8:9);
} }
else else if(iter->second.type() == Link::kUserClosure)
{ {
linkItem->setColor(_loopClosureColor); linkItem->setColor(_loopClosureUserColor);
} }
} else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure)
//rejected loop closures
if(_loopClosureOutlierThr > 0.0f)
{
Transform t = poseA.inverse()*poseB;
if(iter->second.to() != idTo)
{ {
t = t.inverse(); if(_intraInterSessionColors)
}
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); 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); this->setLocalPathColor(color);
} }
else if(r == aChangeGlobalPathColor)
{
this->setGlobalPathColor(color);
}
else if(r == aChangeIntraSessionLoopColor) else if(r == aChangeIntraSessionLoopColor)
{ {
this->setIntraSessionLoopColor(color); this->setIntraSessionLoopColor(color);