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

radiusPosesFiltering default keepLatest=true

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1550 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-07-13 17:59:38 +00:00
parent 6e3303654e
commit 16058dcbbe
4 changed files with 30 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
corelib/src/util3d.cpp
View File

@@ -1793,7 +1793,7 @@ bool loadTOROGraph(const std::string & fileName,
}
std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & poses, float radius, float angle)
std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & poses, float radius, float angle, bool keepLatest)
{
if(poses.size() > 1 && radius > 0.0f && angle>0.0f)
{
@@ -1841,15 +1841,31 @@ std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & p
}
}
bool firstAdded = false;
for(std::set<int>::iterator iter = cloudIndices.begin(); iter!=cloudIndices.end(); ++iter)
if(keepLatest)
{
if(!firstAdded)
bool lastAdded = false;
for(std::set<int>::reverse_iterator iter = cloudIndices.rbegin(); iter!=cloudIndices.rend(); ++iter)
{
indicesKept.insert(*iter);
firstAdded = true;
if(!lastAdded)
{
indicesKept.insert(*iter);
lastAdded = true;
}
indicesChecked.insert(*iter);
}
}
else
{
bool firstAdded = false;
for(std::set<int>::iterator iter = cloudIndices.begin(); iter!=cloudIndices.end(); ++iter)
{
if(!firstAdded)
{
indicesKept.insert(*iter);
firstAdded = true;
}
indicesChecked.insert(*iter);
}
indicesChecked.insert(*iter);
}
}
}