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

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

8
corelib/include/rtabmap/core/Graph.h
View File

@@ -156,6 +156,14 @@ std::multimap<int, int>::iterator RTABMAP_EXP findLink(
std::multimap<int, int> & links,
int from,
int to);
std::multimap<int, Link>::const_iterator RTABMAP_EXP findLink(
const std::multimap<int, Link> & links,
int from,
int to);
std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
const std::multimap<int, int> & links,
int from,
int to);
/**
* Get only the the most recent or older poses in the defined radius.

27
corelib/include/rtabmap/core/util3d_filtering.h
View File

@@ -115,6 +115,33 @@ pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
float radiusSearch,
int minNeighborsInRadius);
/**
* For convenience.
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
float radiusSearch,
int minNeighborsInRadius = 0);
/**
* Subtract a cloud from another one using radius filtering.
* @param cloud the input cloud.
* @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
* @param cloud the input cloud to subtract.
* @param indices the input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
* @param radiusSearch the radius in meter.
* @return the indices of the points satisfying the parameters.
*/
pcl::IndicesPtr RTABMAP_EXP 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 = 0);
/**
* For convenience.
*/

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,