void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// Do data processing here...
// run pass through filter to shrink point cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_passthrough(new pcl::PointCloud<pcl::PointXYZRGB>);
//passthrough_z(input, cloud_passthrough);
passthrough_y(input,cloud_passthrough);
//passthrough_x(cloud_passthrough);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_outlier(new pcl::PointCloud<pcl::PointXYZRGB>);
passthrough_oulier(input,cloud_outlier);
pub_out.publish(*cloud_outlier);
// run ransac to find floor
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZRGB>);
ransac(cloud_passthrough, cloud_projected);
pub.publish(*cloud_projected);
}
pcl::PointCloud<PointT>::Ptr RemovePlane (const pcl::PointCloud<PointT>::Ptr input_cloud)
{
//Passthrough
pcl::PointCloud<PointT>::Ptr cloud_passthrough(new pcl::PointCloud<PointT>);
pcl::PassThrough<PointT> pass;
pass.setInputCloud(input_cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0,2.0);
pass.filter(*cloud_passthrough);
//downsample
pcl::PointCloud<PointT>::Ptr cloud_downsample (new pcl::PointCloud<PointT>);
pcl::VoxelGrid<PointT> vg;
vg.setInputCloud (cloud_passthrough);
vg.setLeafSize (0.01f, 0.01f, 0.01f); //
vg.filter (*cloud_downsample);
pcl::PointCloud<PointT>::Ptr cloud_colorthrough (new pcl::PointCloud<PointT>);
int r,g,b;
for (size_t i = 0; i < cloud_filter->points.size (); i++)
{
r = cloud_filter->points[i].r;
g = cloud_filter->points[i].g;
b = cloud_filter->points[i].b;
if (r > 200 && r < 250)
if (g > 200 && g < 250)
if (b > 200 && b < 250)continue;
cloud_colorthrough->points.push_back (cloud_filter->points[i]);
}
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
return cloud_colorthrough;
}
