i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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;
} }
// return <id, sqrd distance>, including query // return <id, sqrd distance>, excluding query
std::map<int, float> getNodesInRadius( std::map<int, float> getNodesInRadius(
int nodeId, int nodeId,
const std::map<int, Transform> & nodes, const std::map<int, Transform> & nodes,
@@ -883,20 +883,27 @@ std::map<int, float> getNodesInRadius(
float radius) float radius)
{ {
UASSERT(uContains(nodes, nodeId)); 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>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(nodes.size()); cloud->resize(nodes.size()-1);
std::vector<int> ids(nodes.size()); std::vector<int> ids(nodes.size()-1);
int oi = 0; int oi = 0;
for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter) 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()); if(iter->first != nodeId)
ids[oi++] = iter->first; {
(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
ids[oi++] = iter->first;
}
} }
Transform fromT = nodes.at(nodeId); Transform fromT = nodes.at(nodeId);
std::map<int, float> foundNodes;
if(cloud->size()) if(cloud->size())
{ {
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>); 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(); rejectedHypothesis = transform.isNull();
if(rejectedHypothesis) 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) if(!rejectedHypothesis)