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

removing query from poses returned from Graph::getNodesInRadius()

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-09 11:58:06 -04:00
parent 0c55d30f76
commit 49fcf732af
2 changed files with 15 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
corelib/src/Graph.cpp
View File

@@ -875,7 +875,7 @@ int findNearestNode(
return id;
}
// return <id, sqrd distance>, including query
// return <id, sqrd distance>, excluding query
std::map<int, float> getNodesInRadius(
int nodeId,
const std::map<int, Transform> & nodes,
@@ -883,20 +883,27 @@ std::map<int, float> getNodesInRadius(
float radius)
{
UASSERT(uContains(nodes, nodeId));
std::map<int, float> foundNodes;
if(nodes.size() <= 1)
{
return foundNodes;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(nodes.size());
std::vector<int> ids(nodes.size());
cloud->resize(nodes.size()-1);
std::vector<int> ids(nodes.size()-1);
int oi = 0;
for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
{
(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
ids[oi++] = iter->first;
if(iter->first != nodeId)
{
(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
ids[oi++] = iter->first;
}
}
Transform fromT = nodes.at(nodeId);
std::map<int, float> foundNodes;
if(cloud->size())
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);

3
corelib/src/Rtabmap.cpp
View File

@@ -1432,7 +1432,8 @@ bool Rtabmap::process(const SensorData & data)
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)
{
UINFO("Cannot compute a loop closure transform between %d and %d: %s", _loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str());
UWARN("Rejected loop closure transform between %d and %d: %s",
_loopClosureHypothesis.first, signature->id(), rejectedMsg.c_str());
}
}
if(!rejectedHypothesis)