i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

added subtractFiltering() method to filter point clouds by subtracting the previous cloud

Browse Source
This commit is contained in:
matlabbe
2015-06-29 00:19:45 -04:00
parent 5269649661
commit fce1816c21
10 changed files with 341 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
corelib/src/Graph.cpp
View File

@@ -947,6 +947,61 @@ std::multimap<int, int>::iterator findLink(
}
return links.end();
}
std::multimap<int, Link>::const_iterator findLink(
const std::multimap<int, Link> & links,
int from,
int to)
{
std::multimap<int, Link>::const_iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
{
if(iter->second.to() == to)
{
return iter;
}
++iter;
}
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
{
if(iter->second.to() == from)
{
return iter;
}
++iter;
}
return links.end();
}
std::multimap<int, int>::const_iterator findLink(
const std::multimap<int, int> & links,
int from,
int to)
{
std::multimap<int, int>::const_iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
{
if(iter->second == to)
{
return iter;
}
++iter;
}
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
{
if(iter->second == from)
{
return iter;
}
++iter;
}
return links.end();
}
std::map<int, Transform> radiusPosesFiltering(
const std::map<int, Transform> & poses,