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

GraphViewer: fixed duplicated links added to view

Browse Source
This commit is contained in:
matlabbe
2025-06-26 13:30:51 -07:00
parent ee98be9ef7
commit 30962119cb
1 changed files with 16 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
guilib/src/GraphViewer.cpp
View File

@@ -577,33 +577,40 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
for(std::multimap<int, Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{
// make the first id the smallest one
int idFrom = iter->first<iter->second.to()?iter->first:iter->second.to();
int idTo = iter->first<iter->second.to()?iter->second.to():iter->first;
int idFrom = iter->second.from() < iter->second.to() ? iter->second.from() : iter->second.to();
int idTo = iter->second.from() < iter->second.to() ? iter->second.to() : iter->second.from();
if(idFrom == idTo) {
continue;
}
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))
_nodeItems.contains(idFrom) && _nodeItems.contains(idTo))
{
const Transform & poseA = jterA->second;
const Transform & poseB = jterB->second;
QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.end();
if(_linkItems.contains(idFrom))
{
itemIter = _linkItems.find(iter->first);
while(itemIter.key() == idFrom && itemIter != _linkItems.end())
itemIter = _linkItems.find(idFrom);
bool alreadyAdded = false;
while(itemIter != _linkItems.end() && itemIter.key() == idFrom)
{
if(itemIter.value()->to() == idTo && itemIter.value()->type() == iter->second.type())
if(itemIter.value()->to() == idTo && itemIter.value()->isVisible())
{
itemIter.value()->setPoses(poseA, poseB, _viewPlane);
itemIter.value()->show();
linkItem = itemIter.value();
alreadyAdded = true;
break;
}
++itemIter;
}
if(alreadyAdded){
continue;
}
}
bool interSessionClosure = false;