void
pcl::ExtractIndices<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &output)
{
if (indices_->empty ())
{
output.width = output.height = 0;
output.data.clear ();
// If negative, copy all the data
if (negative_)
output = *input_;
return;
}
if (indices_->size () == (input_->width * input_->height))
{
output = *input_;
return;
}
// Copy the common fields (header and fields should have already been copied)
output.is_bigendian = input_->is_bigendian;
output.point_step = input_->point_step;
output.height = 1;
// TODO: check the output cloud and assign is_dense based on whether the points are valid or not
output.is_dense = false;
if (negative_)
{
// Prepare a vector holding all indices
std::vector<int> all_indices (input_->width * input_->height);
for (size_t i = 0; i < all_indices.size (); ++i)
all_indices[i] = i;
std::vector<int> indices = *indices_;
std::sort (indices.begin (), indices.end ());
// Get the diference
std::vector<int> remaining_indices;
set_difference (all_indices.begin (), all_indices.end (), indices.begin (), indices.end (),
inserter (remaining_indices, remaining_indices.begin ()));
// Prepare the output and copy the data
output.width = remaining_indices.size ();
output.data.resize (remaining_indices.size () * output.point_step);
for (size_t i = 0; i < remaining_indices.size (); ++i)
memcpy (&output.data[i * output.point_step], &input_->data[remaining_indices[i] * output.point_step], output.point_step);
}
else
{
// Prepare the output and copy the data
output.width = indices_->size ();
output.data.resize (indices_->size () * output.point_step);
for (size_t i = 0; i < indices_->size (); ++i)
memcpy (&output.data[i * output.point_step], &input_->data[(*indices_)[i] * output.point_step], output.point_step);
}
output.row_step = output.point_step * output.width;
}
void FloorFilter::GetIndicesDifference(size_t cloud_size, const std::vector<int> &indices,
std::vector<int> *difference) {
std::vector<int> all_indices(cloud_size);
for (size_t i = 0; i < all_indices.size(); i++) {
all_indices[i] = i;
}
std::vector<int> copied_indices(indices.begin(), indices.end());
std::sort(copied_indices.begin(), copied_indices.end());
std::set_difference(all_indices.begin(), all_indices.end(),
copied_indices.begin(), copied_indices.end(),
std::back_insert_iterator<std::vector<int> >(*difference));
}
template <typename PointT> void
pcl::ExtractIndices<PointT>::applyFilter (PointCloud &output)
{
if (indices_->empty () || input_->points.empty ())
{
output.width = output.height = 0;
output.points.clear ();
// If negative, copy all the data
if (negative_)
output = *input_;
return;
}
// If the set of indices equals the number of points in the input dataset
if (indices_->size () == (input_->width * input_->height) ||
indices_->size () == input_->points.size ())
{
// If negative, then return an empty cloud
if (negative_)
{
output.width = output.height = 0;
output.points.clear ();
}
// else, we need to return all points
else
output = *input_;
return;
}
// We have a set of indices which is a subset of cloud, so let's start processing
if (negative_)
{
// Prepare a vector holding all indices
std::vector<int> all_indices (input_->points.size ());
for (size_t i = 0; i < all_indices.size (); ++i)
all_indices[i] = i;
std::vector<int> indices = *indices_;
std::sort (indices.begin (), indices.end ());
// Get the diference
std::vector<int> remaining_indices;
set_difference (all_indices.begin (), all_indices.end (), indices.begin (), indices.end (),
inserter (remaining_indices, remaining_indices.begin ()));
copyPointCloud (*input_, remaining_indices, output);
}
else
copyPointCloud (*input_, *indices_, output);
}
void InterpolationSurrogateBuilder<V,M>::sync_data( std::vector<unsigned int>& local_n,
std::vector<double>& local_values,
InterpolationSurrogateData<V,M>& data )
{
// Only members of the inter0comm will do the communication of the local values
unsigned int my_subrank = data.get_paramDomain().env().subRank();
if( my_subrank == 0 )
{
std::vector<double> all_values(data.n_values());
std::vector<unsigned int> all_indices(data.n_values());
std::vector<int> strides;
this->compute_strides( strides );
const MpiComm& inter0comm = data.get_paramDomain().env().inter0Comm();
/*! \todo Would be more efficient to pack local_n and local_values
togethers and do Gatherv only once. */
inter0comm.Gatherv( &local_n[0], local_n.size(), MPI_UNSIGNED,
&all_indices[0], &m_njobs[0], &strides[0], MPI_UNSIGNED,
0 /*root*/,
"InterpolationSurrogateBuilder::sync_data()",
"MpiComm::gatherv() failed!" );
inter0comm.Gatherv( &local_values[0], local_values.size(), MPI_DOUBLE,
&all_values[0], &m_njobs[0], &strides[0], MPI_DOUBLE,
0 /*root*/,
"InterpolationSurrogateBuilder::sync_data()",
"MpiComm::gatherv() failed!" );
// Now set the values.
/* PB: Although we are guaranteed per-rank ordering of the data we gathered,
I'm not sure we can assume the same continuity of the inter0 ranks, i.e.
I'm not sure how QUESO ordered the inter0 ranks. So, we go ahead and
manually set the values. */
if( data.get_paramDomain().env().subRank() == 0 )
{
for( unsigned int n = 0; n < data.n_values(); n++ )
data.set_value( all_indices[n], all_values[n] );
}
}
// Now broadcast the values data to all other processes
data.sync_values( 0 /*root*/);
}
template<typename Point> void
TableObjectCluster<Point>::removeKnownObjects(const PointCloudConstPtr& pc_roi,
std::vector<BoundingBox>& bounding_boxes,
const PointCloudPtr& pc_roi_red)
{
//pcl::copyPointCloud(*pc_roi,pc_roi_red);
boost::shared_ptr<pcl::PointIndices> all_indices(new pcl::PointIndices);
pcl::CropBox<Point> cb;
for(unsigned int i=0; i<bounding_boxes.size(); i++)
{
std::vector<int> indices;
cb.setMin(bounding_boxes[i].min_pt);
cb.setMax(bounding_boxes[i].max_pt);
cb.setTransform(bounding_boxes[i].pose);
cb.filter(indices);
all_indices->indices.insert(all_indices->indices.end(), indices.begin(), indices.end());
}
pcl::ExtractIndices<Point> extract;
extract.setInputCloud(pc_roi);
extract.setIndices(all_indices);
extract.setNegative(true);
extract.filter(*pc_roi_red);
}
void
pcl::ExtractIndices<pcl::PCLPointCloud2>::applyFilter (PCLPointCloud2 &output)
{
if (keep_organized_)
{
output = *input_;
if (negative_)
{
// Prepare the output and copy the data
for (size_t i = 0; i < indices_->size (); ++i)
for (size_t j = 0; j < output.fields.size(); ++j)
memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[j].offset],
&user_filter_value_, sizeof(float));
}
else
{
// Prepare a vector holding all indices
std::vector<int> all_indices (input_->width * input_->height);
for (int i = 0; i < static_cast<int>(all_indices.size ()); ++i)
all_indices[i] = i;
std::vector<int> indices = *indices_;
std::sort (indices.begin (), indices.end ());
// Get the diference
std::vector<int> remaining_indices;
set_difference (all_indices.begin (), all_indices.end (), indices.begin (), indices.end (),
inserter (remaining_indices, remaining_indices.begin ()));
// Prepare the output and copy the data
for (size_t i = 0; i < remaining_indices.size (); ++i)
for (size_t j = 0; j < output.fields.size(); ++j)
memcpy (&output.data[remaining_indices[i] * output.point_step + output.fields[j].offset],
&user_filter_value_, sizeof(float));
}
if (!pcl_isfinite (user_filter_value_))
output.is_dense = false;
return;
}
if (indices_->empty () || (input_->width * input_->height == 0))
{
output.width = output.height = 0;
output.data.clear ();
// If negative, copy all the data
if (negative_)
output = *input_;
return;
}
if (indices_->size () == (input_->width * input_->height))
{
// If negative, then return an empty cloud
if (negative_)
{
output.width = output.height = 0;
output.data.clear ();
}
// else, we need to return all points
else
output = *input_;
return;
}
// Copy the common fields (header and fields should have already been copied)
output.is_bigendian = input_->is_bigendian;
output.point_step = input_->point_step;
output.height = 1;
// TODO: check the output cloud and assign is_dense based on whether the points are valid or not
output.is_dense = false;
if (negative_)
{
// Prepare a vector holding all indices
std::vector<int> all_indices (input_->width * input_->height);
for (int i = 0; i < static_cast<int>(all_indices.size ()); ++i)
all_indices[i] = i;
std::vector<int> indices = *indices_;
std::sort (indices.begin (), indices.end ());
// Get the diference
std::vector<int> remaining_indices;
set_difference (all_indices.begin (), all_indices.end (), indices.begin (), indices.end (),
inserter (remaining_indices, remaining_indices.begin ()));
// Prepare the output and copy the data
output.width = static_cast<uint32_t> (remaining_indices.size ());
output.data.resize (remaining_indices.size () * output.point_step);
for (size_t i = 0; i < remaining_indices.size (); ++i)
memcpy (&output.data[i * output.point_step], &input_->data[remaining_indices[i] * output.point_step], output.point_step);
}
else
{
// Prepare the output and copy the data
output.width = static_cast<uint32_t> (indices_->size ());
output.data.resize (indices_->size () * output.point_step);
for (size_t i = 0; i < indices_->size (); ++i)
memcpy (&output.data[i * output.point_step], &input_->data[(*indices_)[i] * output.point_step], output.point_step);
}
output.row_step = output.point_step * output.width;
}
void MultiPlaneExtraction::extract(const sensor_msgs::PointCloud2::ConstPtr& input,
const jsk_pcl_ros::ClusterPointIndices::ConstPtr& indices,
const jsk_pcl_ros::ModelCoefficientsArray::ConstPtr& coefficients,
const jsk_pcl_ros::PolygonArray::ConstPtr& polygons)
{
boost::mutex::scoped_lock(mutex_);
// convert all to the pcl types
pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::fromROSMsg(*input, *input_cloud);
// concat indices into one PointIndices
pcl::PointIndices::Ptr all_indices (new pcl::PointIndices);
for (size_t i = 0; i < indices->cluster_indices.size(); i++) {
std::vector<int> one_indices = indices->cluster_indices[i].indices;
for (size_t j = 0; j < one_indices.size(); j++) {
all_indices->indices.push_back(one_indices[j]);
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr nonplane_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::ExtractIndices<pcl::PointXYZRGB> extract_nonplane;
extract_nonplane.setNegative(true);
extract_nonplane.setInputCloud(input_cloud);
extract_nonplane.setIndices(all_indices);
extract_nonplane.filter(*nonplane_cloud);
nonplane_pub_.publish(nonplane_cloud);
// for each plane, project nonplane_cloud to the plane and find the points
// inside of the polygon
std::set<int> result_set;
for (size_t plane_i = 0; plane_i < coefficients->coefficients.size(); plane_i++) {
pcl::ExtractPolygonalPrismData<pcl::PointXYZRGB> prism_extract;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr hull_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
geometry_msgs::Polygon the_polygon = polygons->polygons[plane_i].polygon;
for (size_t i = 0; i < the_polygon.points.size(); i++) {
pcl::PointXYZRGB p;
p.x = the_polygon.points[i].x;
p.y = the_polygon.points[i].y;
p.z = the_polygon.points[i].z;
hull_cloud->points.push_back(p);
}
prism_extract.setInputCloud(nonplane_cloud);
prism_extract.setHeightLimits(min_height_, max_height_);
prism_extract.setInputPlanarHull(hull_cloud);
//pcl::PointCloud<pcl::PointXYZRGB> output;
pcl::PointIndices output_indices;
prism_extract.segment(output_indices);
// append output to result_cloud
for (size_t i = 0; i < output_indices.indices.size(); i++) {
result_set.insert(output_indices.indices[i]);
//result_cloud.points.push_back(output.points[i]);
}
}
// convert std::set to PCLIndicesMsg
//PCLIndicesMsg output_indices;
pcl::PointCloud<pcl::PointXYZRGB> result_cloud;
pcl::PointIndices::Ptr all_result_indices (new pcl::PointIndices());
for (std::set<int>::iterator it = result_set.begin();
it != result_set.end();
it++) {
all_result_indices->indices.push_back(*it);
}
pcl::ExtractIndices<pcl::PointXYZRGB> extract_all_indices;
extract_all_indices.setInputCloud(nonplane_cloud);
extract_all_indices.setIndices(all_result_indices);
extract_all_indices.filter(result_cloud);
sensor_msgs::PointCloud2 ros_result;
pcl::toROSMsg(result_cloud, ros_result);
ros_result.header = input->header;
pub_.publish(ros_result);
}
