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

DbViewer: sorting loop closures by parent ids. When checking ignore pose correction, graph is optimized without them.

Browse Source
This commit is contained in:
matlabbe
2017-10-05 17:10:15 -04:00
parent c4ae4919a5
commit 0b8ff7cc01
1 changed files with 16 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
guilib/src/DatabaseViewer.cpp
View File

@@ -1331,6 +1331,7 @@ void DatabaseViewer::updateIds()
std::multimap<int, Link> links; std::multimap<int, Link> links;
dbDriver_->getAllLinks(links, true); dbDriver_->getAllLinks(links, true);
UDEBUG("%d total links loaded", (int)links.size()); UDEBUG("%d total links loaded", (int)links.size());
double totalOdom = 0.0; double totalOdom = 0.0;
Transform previousPose; Transform previousPose;
int sessions = ids_.size()?1:0; int sessions = ids_.size()?1:0;
@@ -1442,6 +1443,7 @@ void DatabaseViewer::updateIds()
} }
} }
} }
if(!groundTruthPoses_.empty() || !gpsPoses_.empty()) if(!groundTruthPoses_.empty() || !gpsPoses_.empty())
{ {
ui_->checkBox_alignPosesWithGroundTruth->setVisible(true); ui_->checkBox_alignPosesWithGroundTruth->setVisible(true);
@@ -4606,11 +4608,10 @@ void DatabaseViewer::updateGraphView()
} }
} }
links = updateLinksWithModifications(links);
if(ui_->checkBox_ignorePoseCorrection->isChecked()) if(ui_->checkBox_ignorePoseCorrection->isChecked())
{ {
std::multimap<int, Link> tmp = links; for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
std::multimap<int, Link>::iterator jter=links.begin();
for(std::multimap<int, Link>::iterator iter=tmp.begin(); iter!=tmp.end(); ++iter, ++jter)
{ {
if(iter->second.type() == Link::kNeighbor || if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged) iter->second.type() == Link::kNeighborMerged)
@@ -4628,11 +4629,6 @@ void DatabaseViewer::updateGraphView()
} }
} }
} }
links = updateLinksWithModifications(tmp);
}
else
{
links = updateLinksWithModifications(links);
} }
// filter links // filter links
@@ -5455,7 +5451,19 @@ void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
loopLinks_.clear(); loopLinks_.clear();
std::multimap<int, Link> links = updateLinksWithModifications(links_); std::multimap<int, Link> links = updateLinksWithModifications(links_);
int position = ui_->horizontalSlider_loops->value(); int position = ui_->horizontalSlider_loops->value();
std::multimap<int, Link> linksSortedByParents;
for(std::multimap<int, rtabmap::Link>::iterator iter = links.begin(); iter!=links.end(); ++iter) for(std::multimap<int, rtabmap::Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.to() > iter->second.from())
{
linksSortedByParents.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
}
else
{
linksSortedByParents.insert(*iter);
}
}
for(std::multimap<int, rtabmap::Link>::iterator iter = linksSortedByParents.begin(); iter!=linksSortedByParents.end(); ++iter)
{ {
if(!iter->second.transform().isNull()) if(!iter->second.transform().isNull())
{ {