i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-09 03:27:45 +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
+15 -12
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) 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;
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, fromId, toId); if(posesIn.find(toId) != posesIn.end())
int nextDepth = toId!=fromId?depth-1:depth;
if(depth == 0 || d < nextDepth || curentPoses.find(toId) != curentPoses.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(!uContains(posesOut, toId))
if(curentPoses.find(toId) == curentPoses.end())
{ {
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);
} }
} }
} }