i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Optimizer:getConnectedGraph() don't add poses not included in the input poses

Browse Source
This commit is contained in:
matlabbe
2018-11-09 17:14:09 -05:00
parent c43bd6cd3f
commit 3bb874825f
1 changed files with 15 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
corelib/src/Optimizer.cpp
View File

@@ -202,22 +202,25 @@ void Optimizer::getConnectedGraph(
for(std::multimap<int, int>::const_iterator iter=biLinks.find(fromId); iter!=biLinks.end() && iter->first==fromId; ++iter)
{
int toId = iter->second;
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, fromId, toId);
int nextDepth = toId!=fromId?depth-1:depth;
if(depth == 0 || d < nextDepth || curentPoses.find(toId) != curentPoses.end())
if(posesIn.find(toId) != posesIn.end())
{
if(!uContains(posesOut, toId))
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, fromId, toId);
int nextDepth = toId!=fromId?depth-1:depth;
if(depth == 0 || d < nextDepth || curentPoses.find(toId) != curentPoses.end())
{
posesOut.insert(std::make_pair(toId, posesOut.at(fromId) * (kter->second.from()==fromId?kter->second.transform():kter->second.transform().inverse())));
if(curentPoses.find(toId) == curentPoses.end())
if(!uContains(posesOut, toId))
{
nextPoses.insert(toId);
posesOut.insert(std::make_pair(toId, posesOut.at(fromId) * (kter->second.from()==fromId?kter->second.transform():kter->second.transform().inverse())));
if(curentPoses.find(toId) == curentPoses.end())
{
nextPoses.insert(toId);
}
}
if(graph::findLink(linksOut, fromId, toId) == linksOut.end())
{
// only add unique links
linksOut.insert(*kter);
}
}
if(graph::findLink(linksOut, fromId, toId) == linksOut.end())
{
// only add unique links
linksOut.insert(*kter);
}
}
}