i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 18:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Optimizer: fixed some constraints between same nodes with different type ignored. GraphViewer: fixed display of links between same nodes with different type

Browse Source
This commit is contained in:
matlabbe
2023-11-12 17:14:17 -08:00
parent 56552afa6c
commit 4812ce4eaf
4 changed files with 90 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Optimizer.cpp
+9 -8
View File
@@ -202,15 +202,15 @@ void Optimizer::getConnectedGraph(
std::set<int> nextPoses;
nextPoses.insert(fromId);
std::multimap<int, int> biLinks;
std::multimap<int, std::pair<int, Link::Type> > biLinks;
for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
{
if(iter->second.from() != iter->second.to())
{
if(graph::findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end())
if(graph::findLink(biLinks, iter->second.from(), iter->second.to(), true, iter->second.type()) == biLinks.end())
{
biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
biLinks.insert(std::make_pair(iter->second.from(), std::make_pair(iter->second.to(), iter->second.type())));
biLinks.insert(std::make_pair(iter->second.to(), std::make_pair(iter->second.from(), iter->second.type())));
}
}
}
@@ -234,12 +234,13 @@ void Optimizer::getConnectedGraph(
}
}
for(std::multimap<int, int>::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
for(std::multimap<int, std::pair<int, Link::Type> >::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
{
int toId = iter->second;
int toId = iter->second.first;
Link::Type type = iter->second.second;
if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
{
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId);
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId, true, type);
if(nextPoses.find(toId) == nextPoses.end())
{
if(!uContains(posesOut, toId))
@@ -282,7 +283,7 @@ void Optimizer::getConnectedGraph(
}
// only add unique links
if(graph::findLink(linksOut, currentId, toId) == linksOut.end())
if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
{
if(kter->second.to() < 0)
{