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

fixed mapId for pose -1 not found error when filtering nodes to be shown

Browse Source
This commit is contained in:
matlabbe
2017-10-29 13:23:38 -04:00
parent 85273b9ed7
commit 426a2f983c
1 changed files with 16 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
guilib/src/MainWindow.cpp
View File

@@ -1991,7 +1991,17 @@ void MainWindow::updateMapCloud(
{ {
float radius = _preferencesDialog->getCloudFilteringRadius(); float radius = _preferencesDialog->getCloudFilteringRadius();
float angle = _preferencesDialog->getCloudFilteringAngle()*CV_PI/180.0; // convert to rad float angle = _preferencesDialog->getCloudFilteringAngle()*CV_PI/180.0; // convert to rad
poses = rtabmap::graph::radiusPosesFiltering(posesIn, radius, angle); bool hasNeg = posesIn.find(-1) != posesIn.end();
if(hasNeg)
{
std::map<int, Transform> posesInTmp = posesIn;
posesInTmp.erase(-1);
poses = rtabmap::graph::radiusPosesFiltering(posesIn, radius, angle);
}
else
{
poses = rtabmap::graph::radiusPosesFiltering(posesIn, radius, angle);
}
for(std::map<int, Transform>::iterator iter= poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::iterator iter= poses.begin(); iter!=poses.end(); ++iter)
{ {
std::map<int, int>::const_iterator jter = mapIdsIn.find(iter->first); std::map<int, int>::const_iterator jter = mapIdsIn.find(iter->first);
@@ -2004,6 +2014,11 @@ void MainWindow::updateMapCloud(
UERROR("map id of node %d not found!", iter->first); UERROR("map id of node %d not found!", iter->first);
} }
} }
//keep -1
if(hasNeg)
{
poses.insert(*posesIn.find(-1));
}
if(verboseProgress) if(verboseProgress)
{ {