i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

DatabaseViewer: Added total path length under the Graph view

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1342 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-06-09 22:05:52 +00:00
parent 35db51419e
commit fc59a4bb9c
3 changed files with 41 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
guilib/src/DatabaseViewer.cpp
+25 -2
View File
@@ -365,6 +365,8 @@ void DatabaseViewer::updateIds()
{
ui_->horizontalSlider_loops->setEnabled(false);
}
updateGraphView();
}
void DatabaseViewer::generateGraph()
@@ -907,9 +909,30 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
}
UINFO("Update scans list... done");
}
ui_->graphViewer->updateGraph(uValueAt(graphes_, value), links_, scans_);
std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, value);
ui_->graphViewer->updateGraph(graph, links_, scans_);
ui_->label_iterations->setNum(value);
//compute total length (neighbor links)
float length = 0.0f;
for(std::multimap<int, rtabmap::Link>::const_iterator iter=links_.begin(); iter!=links_.end(); ++iter)
{
std::map<int, rtabmap::Transform>::const_iterator jterA = graph.find(iter->first);
std::map<int, rtabmap::Transform>::const_iterator jterB = graph.find(iter->second.to());
if(jterA != graph.end() && jterB != graph.end())
{
const rtabmap::Transform & poseA = jterA->second;
const rtabmap::Transform & poseB = jterB->second;
if(iter->second.type() == rtabmap::Link::kNeighbor)
{
Eigen::Vector3f vA, vB;
poseA.getTranslation(vA[0], vA[1], vA[2]);
poseB.getTranslation(vB[0], vB[1], vB[2]);
length += (vB - vA).norm();
}
}
}
ui_->label_pathLength->setNum(length);
}
}
void DatabaseViewer::updateGraphView()