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

Added some asserts on pose radius filtering

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-31 10:48:43 -04:00
parent 23a885f05b
commit 7949ba74b2
1 changed files with 13 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
corelib/src/Graph.cpp
View File

@@ -899,9 +899,10 @@ std::map<int, Transform> radiusPosesFiltering(
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(poses.size()); cloud->resize(poses.size());
int i=0; int i=0;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter, ++i)
{ {
(*cloud)[i++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()); (*cloud)[i] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
UASSERT_MSG(pcl::isFinite((*cloud)[i]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
} }
// radius filtering // radius filtering
@@ -1003,9 +1004,10 @@ std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & p
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(poses.size()); cloud->resize(poses.size());
int i=0; int i=0;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter, ++i)
{ {
(*cloud)[i++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()); (*cloud)[i] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
UASSERT_MSG(pcl::isFinite((*cloud)[i]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
} }
// radius clustering (nearest neighbors) // radius clustering (nearest neighbors)
@@ -1249,17 +1251,21 @@ std::map<int, float> getNodesInRadius(
} }
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()-1); cloud->resize(nodes.size());
std::vector<int> ids(nodes.size()-1); std::vector<int> ids(nodes.size());
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)
{ {
if(iter->first != nodeId) if(iter->first != nodeId)
{ {
(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()); (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
ids[oi++] = iter->first; UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
ids[oi] = iter->first;
++oi;
} }
} }
cloud->resize(oi);
ids.resize(oi);
Transform fromT = nodes.at(nodeId); Transform fromT = nodes.at(nodeId);