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

Rtabmap: added getNodesInRadius() public functions

Browse Source
This commit is contained in:
matlabbe
2020-09-28 12:03:42 -04:00
parent 933ac736f1
commit bedc771fa4
5 changed files with 102 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
guilib/src/MainWindow.cpp
View File

@@ -2318,12 +2318,12 @@ void MainWindow::updateMapCloud(
int maxNodes = uStr2Int(_preferencesDialog->getParameter(Parameters::kGridGlobalMaxNodes()));
if(maxNodes > 0 && poses.size()>1)
{
std::vector<int> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxNodes);
std::map<int, float> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxNodes);
std::map<int, Transform> nearestPoses;
nearestPoses.insert(*poses.rbegin());
for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
std::map<int, Transform>::iterator pter = poses.find(*iter);
std::map<int, Transform>::iterator pter = poses.find(iter->first);
if(pter != poses.end())
{
nearestPoses.insert(*pter);