i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-10 11:59:50 +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
corelib/src/Optimizer.cpp
+3
View File
@@ -202,6 +202,8 @@ void Optimizer::getConnectedGraph(
for(std::multimap<int, int>::const_iterator iter=biLinks.find(fromId); iter!=biLinks.end() && iter->first==fromId; ++iter) for(std::multimap<int, int>::const_iterator iter=biLinks.find(fromId); iter!=biLinks.end() && iter->first==fromId; ++iter)
{ {
int toId = iter->second; int toId = iter->second;
if(posesIn.find(toId) != posesIn.end())
{
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, fromId, toId); std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, fromId, toId);
int nextDepth = toId!=fromId?depth-1:depth; int nextDepth = toId!=fromId?depth-1:depth;
if(depth == 0 || d < nextDepth || curentPoses.find(toId) != curentPoses.end()) if(depth == 0 || d < nextDepth || curentPoses.find(toId) != curentPoses.end())
@@ -222,6 +224,7 @@ void Optimizer::getConnectedGraph(
} }
} }
} }
}
++d; ++d;
} }
} }