i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

point_cloud_xyz: doing radius filtering AFTER voxel filtering to gain a lot of performance

Browse Source
This commit is contained in:
Mathieu Labbe
2016-05-19 15:11:46 -04:00
parent 440e8acdc6
commit d08a87c38b
2 changed files with 34 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/point_cloud_xyz.cpp
+17 -18
View File
@@ -270,25 +270,9 @@ private:
void processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, const std_msgs::Header & header) void processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, const std_msgs::Header & header)
{ {
if(pclCloud->size()) if(pclCloud->size() && (minDepth_ != 0.0 || maxDepth_ > minDepth_))
{ {
if(minDepth_ != 0.0 || maxDepth_ > minDepth_) pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", minDepth_, maxDepth_>minDepth_?maxDepth_:std::numeric_limits<float>::max());
{
pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", minDepth_, maxDepth_>minDepth_?maxDepth_:std::numeric_limits<float>::max());
}
else if(noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
// remvoe NaN values
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
}
}
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
} }
if(pclCloud->size() && voxelSize_ > 0.0) if(pclCloud->size() && voxelSize_ > 0.0)
@@ -296,6 +280,21 @@ private:
pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_); pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
} }
// Do radius filtering after voxel filtering ( a lot faster)
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
if(voxelSize_ <= 0.0 && !(minDepth_ != 0.0 || maxDepth_ > minDepth_))
{
// remove NaN values
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
}
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
}
sensor_msgs::PointCloud2 rosCloud; sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(*pclCloud, rosCloud); pcl::toROSMsg(*pclCloud, rosCloud);
rosCloud.header.stamp = header.stamp; rosCloud.header.stamp = header.stamp;
src/nodelets/point_cloud_xyzrgb.cpp
+17 -18
View File
@@ -310,25 +310,9 @@ private:
void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud, const std_msgs::Header & header) void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::Ptr & pclCloud, const std_msgs::Header & header)
{ {
if(pclCloud->size()) if(pclCloud->size() && (minDepth_ != 0.0 || maxDepth_ > minDepth_))
{ {
if(minDepth_ != 0.0 || maxDepth_ > minDepth_) pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", minDepth_, maxDepth_>minDepth_?maxDepth_:std::numeric_limits<float>::max());
{
pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", minDepth_, maxDepth_>minDepth_?maxDepth_:std::numeric_limits<float>::max());
}
else if(noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
// remvoe NaN values
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
}
}
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
} }
if(pclCloud->size() && voxelSize_ > 0.0) if(pclCloud->size() && voxelSize_ > 0.0)
@@ -336,6 +320,21 @@ private:
pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_); pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
} }
// Do radius filtering after voxel filtering ( a lot faster)
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
if(voxelSize_ <= 0.0 && !(minDepth_ != 0.0 || maxDepth_ > minDepth_))
{
// remove NaN values
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
}
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
}
sensor_msgs::PointCloud2 rosCloud; sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(*pclCloud, rosCloud); pcl::toROSMsg(*pclCloud, rosCloud);
rosCloud.header.stamp = header.stamp; rosCloud.header.stamp = header.stamp;