int process(const tendrils& inputs, const tendrils& outputs,
boost::shared_ptr<const ::pcl::PointCloud<Point> >& input)
{
// initialize outputs and filter
typename ::pcl::PointCloud<Point>::Ptr output(new typename ::pcl::PointCloud<Point>);
::pcl::ExtractIndices<Point> filter;
filter.setInputCloud(input);
output->header = input->header;
// Extract location of rgb (similar to pcl::PackedRGBComparison<T>)
#if PCL_VERSION_COMPARE(<,1,7,0)
std::vector<sensor_msgs::PointField> fields;
#else
typedef ::pcl::PCLPointField PCLPointField;
std::vector<PCLPointField> fields;
#endif
::pcl::getFields(*input, fields);
size_t idx;
for (idx = 0; idx < fields.size(); idx++)
{
if ( fields[idx].name == "rgb" || fields[idx].name == "rgba" )
break;
}
if (idx == fields.size())
{
throw std::runtime_error("[ColorizeClouds] requires an rgb or rgba field.");
return -1;
}
for (size_t i = 0; i < clusters_->size(); i++)
{
::pcl::PointCloud<Point> cloud;
// extract indices into a cloud
filter.setIndices( ::pcl::PointIndicesPtr( new ::pcl::PointIndices ((*clusters_)[i])) );
filter.filter(cloud);
float hue = (360.0 / clusters_->size()) * i;
float r, g, b;
hsv2rgb(hue, *saturation_, *value_, r, g, b);
// colorize cloud
for (size_t j = 0; j < cloud.points.size(); j++)
{
Point &p = cloud.points[j];
unsigned char* pt_rgb = (unsigned char*) &p;
pt_rgb += fields[idx].offset;
(*pt_rgb) = (unsigned char) (r * 255);
(*(pt_rgb+1)) = (unsigned char) (g * 255);
(*(pt_rgb+2)) = (unsigned char) (b * 255);
}
// append
cloud.header = input->header;
*output += cloud;
}
*output_ = xyz_cloud_variant_t(output);
return OK;
}
int process(const tendrils& inputs, const tendrils& outputs,
boost::shared_ptr<const ::pcl::PointCloud<Point> >& input1,
boost::shared_ptr<const ::pcl::PointCloud<Point> >& input2)
{
::pcl::PointCloud<Point> cloud;
cloud = *input1;
cloud += *input2;
*output_ = xyz_cloud_variant_t(cloud.makeShared());
return 0;
}
int process(const tendrils& inputs, const tendrils& outputs,
boost::shared_ptr<const ::pcl::PointCloud<Point> >& input)
{
::pcl::PointCloud<Point> cloud;
*output_ = xyz_cloud_variant_t(cloud.makeShared());
::pcl::StatisticalOutlierRemoval<Point> filter;
if(input->size() < 1)
return ecto::OK;
filter.setInputCloud(input);
filter.setMeanK(*mean_k_);
filter.setStddevMulThresh(*stddev_);
filter.setNegative(*negative_);
filter.filter(cloud);
cloud.header = input->header;
*output_ = xyz_cloud_variant_t(cloud.makeShared());
return OK;
}
int process(const tendrils& inputs, const tendrils& outputs,
boost::shared_ptr<const ::pcl::PointCloud<Point> >& input)
{
::pcl::ConvexHull<Point> filter;
::pcl::PointCloud<Point> cloud;
filter.setInputCloud(input);
filter.reconstruct(cloud);
*output_ = xyz_cloud_variant_t(cloud.makeShared());
return ecto::OK;
}
int process(const tendrils& inputs, const tendrils& outputs,
boost::shared_ptr<const ::pcl::PointCloud<Point> >& input)
{
::pcl::RadiusOutlierRemoval<Point> filter;
filter.setMinNeighborsInRadius(*min_);
filter.setInputCloud(input);
::pcl::PointCloud<Point> cloud;
filter.filter(cloud);
cloud.header = input->header;
*output_ = xyz_cloud_variant_t(cloud.makeShared());
return OK;
}
int process(const tendrils& inputs, const tendrils& outputs,
boost::shared_ptr<const ::pcl::PointCloud<Point> >& input)
{
::pcl::ConvexHull<Point> filter;
typename ::pcl::PointCloud<Point>::Ptr cloud(new typename ::pcl::PointCloud<Point>);
filter.setInputCloud(input);
if(indices_.user_supplied())
filter.setIndices(*indices_);
filter.setDimension(*dimensionality_);
filter.reconstruct(*cloud);
*output_ = xyz_cloud_variant_t(cloud);
return ecto::OK;
}
int process(const tendrils& inputs, const tendrils& outputs,
boost::shared_ptr<const ::pcl::PointCloud<Point> >& input)
{
::pcl::ExtractIndices<Point> filter;
filter.setNegative(*negative_);
filter.setIndices(*indices_);
filter.setInputCloud(input);
typename ::pcl::PointCloud<Point>::Ptr cloud(new typename ::pcl::PointCloud<Point>);
filter.filter(*cloud);
cloud->header = input->header;
*output_ = xyz_cloud_variant_t(cloud);
return OK;
}
int process(const tendrils& inputs, const tendrils& outputs,
boost::shared_ptr<const ::pcl::PointCloud<Point> >& input)
{
::pcl::PassThrough<Point> filter;
filter.setFilterFieldName(*filter_field_name_);
filter.setFilterLimits(*filter_limit_min_, *filter_limit_max_);
filter.setFilterLimitsNegative(*filter_limit_negative_);
filter.setInputCloud(input);
::pcl::PointCloud<Point> cloud;
filter.filter(cloud);
cloud.header = input->header;
*output_ = xyz_cloud_variant_t(cloud.makeShared());
return OK;
}
int process(const tendrils& inputs, const tendrils& outputs,
boost::shared_ptr<const ::pcl::PointCloud<Point> >& input)
{
::pcl::ExtractIndices<Point> filter;
int largest = 0;
for (size_t i = 0; i < clusters_->size(); i++)
{
if( (*clusters_)[i].indices.size() > (*clusters_)[largest].indices.size() )
{
largest = i;
}
}
filter.setIndices( ::pcl::PointIndicesPtr( new ::pcl::PointIndices ((*clusters_)[largest])) );
filter.setInputCloud(input);
::pcl::PointCloud<Point> cloud;
filter.filter(cloud);
cloud.header = input->header;
*output_ = xyz_cloud_variant_t(cloud.makeShared());
return OK;
}
