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 subtractFiltering() method to filter point clouds by subtracting the previous cloud

Browse Source
This commit is contained in:
matlabbe
2015-06-29 00:19:45 -04:00
parent 5269649661
commit fce1816c21
10 changed files with 341 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
corelib/src/Graph.cpp
View File

@@ -947,6 +947,61 @@ std::multimap<int, int>::iterator findLink(
}
return links.end();
}
std::multimap<int, Link>::const_iterator findLink(
const std::multimap<int, Link> & links,
int from,
int to)
{
std::multimap<int, Link>::const_iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
{
if(iter->second.to() == to)
{
return iter;
}
++iter;
}
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
{
if(iter->second.to() == from)
{
return iter;
}
++iter;
}
return links.end();
}
std::multimap<int, int>::const_iterator findLink(
const std::multimap<int, int> & links,
int from,
int to)
{
std::multimap<int, int>::const_iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
{
if(iter->second == to)
{
return iter;
}
++iter;
}
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
{
if(iter->second == from)
{
return iter;
}
++iter;
}
return links.end();
}
std::map<int, Transform> radiusPosesFiltering(
const std::map<int, Transform> & poses,

76
corelib/src/util3d_filtering.cpp
View File

@@ -281,6 +281,82 @@ pcl::IndicesPtr radiusFiltering(
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, minNeighborsInRadius);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out;
}
pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>(false));
if(indices->size())
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
if(substractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
}
else
{
tree->setInputCloud(substractCloud);
}
for(unsigned int i=0; i<indices->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
if(k <= minNeighborsInRadius)
{
output->at(oi++) = indices->at(i);
}
}
output->resize(oi);
return output;
}
else
{
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator
if(substractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
}
else
{
tree->setInputCloud(substractCloud);
}
for(unsigned int i=0; i<cloud->size(); ++i)
{
std::vector<int> kIndices;
std::vector<float> kDistances;
int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
if(k <= minNeighborsInRadius)
{
output->at(oi++) = i;
}
}
output->resize(oi);
return output;
}
}
pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,