void FloorFilter::PublishCloudFromIndices(const pcl::PointCloud<pcl::PointXYZ> &cloud,
const std::vector<int> &indices,
ros::Publisher &publisher) {
pcl::PointCloud<pcl::PointXYZ>::Ptr indices_cloud(new pcl::PointCloud<pcl::PointXYZ>);
MakeCloudFromIndices(cloud, indices, indices_cloud.get());
publisher.publish(indices_cloud);
}
std::vector<GraspHypothesis> HandSearch::findHands(const PointCloud::Ptr cloud,
const Eigen::VectorXi& pts_cam_source, const std::vector<int>& indices,
const PointCloud::Ptr cloud_plot, bool calculates_antipodal,
bool uses_clustering)
{
// create KdTree for neighborhood search
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloud);
cloud_normals_.resize(3, cloud->size());
cloud_normals_.setZero(3, cloud->size());
// calculate normals for all points
if (calculates_antipodal)
{
//std::cout << "Calculating normals for all points\n";
nn_radius_taubin_ = 0.01;
std::vector<int> indices_cloud(cloud->size());
for (int i = 0; i < indices_cloud.size(); i++)
indices_cloud[i] = i;
findQuadrics(cloud, pts_cam_source, kdtree, indices_cloud);
nn_radius_taubin_ = 0.03;
}
// draw samples from the point cloud uniformly
std::vector<int> indices_rand;
Eigen::VectorXi hands_cam_source;
if (indices.size() == 0)
{
double t_rand = omp_get_wtime();
//std::cout << "Generating uniform random indices ...\n";
indices_rand.resize(num_samples_);
pcl::RandomSample<pcl::PointXYZ> random_sample;
random_sample.setInputCloud(cloud);
random_sample.setSample(num_samples_);
random_sample.filter(indices_rand);
hands_cam_source.resize(num_samples_);
for (int i = 0; i < num_samples_; i++)
hands_cam_source(i) = pts_cam_source(indices_rand[i]);
//std::cout << " Done in " << omp_get_wtime() - t_rand << " sec\n";
}
else
indices_rand = indices;
if (plots_samples_)
plot_.plotSamples(indices_rand, cloud);
// find quadrics
//std::cout << "Estimating local axes ...\n";
std::vector<Quadric> quadric_list = findQuadrics(cloud, pts_cam_source, kdtree, indices_rand);
if (plots_local_axes_)
plot_.plotLocalAxes(quadric_list, cloud_plot);
// find hands
//std::cout << "Finding hand poses ...\n";
std::vector<GraspHypothesis> hand_list = findHands(cloud, pts_cam_source, quadric_list, hands_cam_source, kdtree);
return hand_list;
}
