affordances.cpp
Go to the documentation of this file.
2 
3 typedef pcl::PointCloud<pcl::PointXYZRGBA> PointCloud;
4 
5 const int TAUBIN = 0;
6 const int PCA = 1;
7 const int NORMALS = 2;
8 
9 const std::string CURVATURE_ESTIMATORS[] = {"Taubin", "PCA", "Normals"};
10 
12 const int Affordances::NUM_SAMPLES = 5000;
14 const double Affordances::NEIGHBOR_RADIUS = 0.025;
15 const int Affordances::MAX_NUM_IN_FRONT = 20;
16 const double Affordances::TARGET_RADIUS = 0.08;
17 const double Affordances::RADIUS_ERROR = 0.013;
18 const double Affordances::HANDLE_GAP = 0.08;
19 const double Affordances::MAX_RANGE = 1.0;
20 const bool Affordances::USE_CLEARANCE_FILTER = true;
21 const bool Affordances::USE_OCCLUSION_FILTER = true;
22 const int Affordances::ALIGNMENT_RUNS = 3;
24 const double Affordances::ALIGNMENT_DIST_RADIUS = 0.02;
25 const double Affordances::ALIGNMENT_ORIENT_RADIUS = 0.1;
26 const double Affordances::ALIGNMENT_RADIUS_RADIUS = 0.003;
27 const double Affordances::WORKSPACE_MIN = -1.0;
28 const double Affordances::WORKSPACE_MAX = 1.0;
29 
31 {
32  // read parameters from ROS launch file
33  std::string file_default = "";
34  node.param("file", this->file, file_default);
35  node.param("target_radius", this->target_radius, this->TARGET_RADIUS);
36  node.param("target_radius_error", this->radius_error, this->RADIUS_ERROR);
37  node.param("affordance_gap", this->handle_gap, this->HANDLE_GAP);
38  node.param("sample_size", this->num_samples, this->NUM_SAMPLES);
39  node.param("max_range", this->max_range, this->MAX_RANGE);
40  node.param("use_clearance_filter", this->use_clearance_filter, this->USE_CLEARANCE_FILTER);
41  node.param("use_occlusion_filter", this->use_occlusion_filter, this->USE_OCCLUSION_FILTER);
42  node.param("curvature_estimator", this->curvature_estimator, this->CURVATURE_ESTIMATOR);
43  node.param("alignment_runs", this->alignment_runs, this->ALIGNMENT_RUNS);
44  node.param("alignment_min_inliers", this->alignment_min_inliers, this->ALIGNMENT_MIN_INLIERS);
45  node.param("alignment_dist_radius", this->alignment_dist_radius, this->ALIGNMENT_DIST_RADIUS);
46  node.param("alignment_orient_radius", this->alignment_orient_radius, this->ALIGNMENT_ORIENT_RADIUS);
47  node.param("alignment_radius_radius", this->alignment_radius_radius, this->ALIGNMENT_RADIUS_RADIUS);
48  node.param("workspace_min_x", this->workspace_limits.min_x, this->WORKSPACE_MIN);
49  node.param("workspace_max_x", this->workspace_limits.max_x, this->WORKSPACE_MAX);
50  node.param("workspace_min_y", this->workspace_limits.min_y, this->WORKSPACE_MIN);
51  node.param("workspace_max_y", this->workspace_limits.max_y, this->WORKSPACE_MAX);
52  node.param("workspace_min_z", this->workspace_limits.min_z, this->WORKSPACE_MIN);
53  node.param("workspace_max_z", this->workspace_limits.max_z, this->WORKSPACE_MAX);
54  node.param("num_threads", this->num_threads, 1);
55 
56  // print parameters
57  printf("PARAMETERS\n");
58  printf(" file: %s\n", this->file.c_str());
59  printf(" target radius: %.3f\n", this->target_radius);
60  printf(" target radius error: %.3f\n", this->radius_error);
61  printf(" min. affordance gap: %.3f\n", this->handle_gap);
62  printf(" number of samples: %i\n", this->num_samples);
63  printf(" max. range: %.3f\n", this->max_range);
64  printf(" use clearance filter: %s\n", this->use_clearance_filter ? "true" : "false");
65  printf(" use occlusion filter: %s\n", this->use_occlusion_filter ? "true" : "false");
66  printf(" curvature estimator: %s\n", CURVATURE_ESTIMATORS[this->curvature_estimator].c_str());
67  printf(" number of alignment runs: %i\n", this->alignment_runs);
68  printf(" min. number of alignment inliers: %i\n", this->alignment_min_inliers);
69  printf(" alignment distance threshold: %.3f\n", this->alignment_dist_radius);
70  printf(" alignment orientation threshold: %.3f\n", this->alignment_orient_radius);
71  printf(" alignment radius threshold: %.3f\n", this->alignment_radius_radius);
72  printf(" workspace_min_x: %.3f\n", this->workspace_limits.min_x);
73  printf(" workspace_max_x: %.3f\n", this->workspace_limits.max_x);
74  printf(" workspace_min_y: %.3f\n", this->workspace_limits.min_y);
75  printf(" workspace_max_y: %.3f\n", this->workspace_limits.max_y);
76  printf(" workspace_min_z: %.3f\n", this->workspace_limits.min_z);
77  printf(" workspace_max_z: %.3f\n", this->workspace_limits.max_z);
78  printf(" num_threads: %i\n", this->num_threads);
79 }
80 
81 PointCloud::Ptr Affordances::maxRangeFilter(const PointCloud::Ptr &cloud_in)
82 {
83  PointCloud::Ptr cloud_out(new PointCloud);
84 
85  for (std::size_t i = 0; i < cloud_in->points.size(); i++)
86  {
87  if (cloud_in->points[i].x * cloud_in->points[i].x + cloud_in->points[i].y * cloud_in->points[i].y
88  + cloud_in->points[i].z * cloud_in->points[i].z < this->max_range * this->max_range)
89  cloud_out->points.push_back(cloud_in->points[i]);
90  }
91 
92  return cloud_out;
93 }
94 
95 bool Affordances::isPointInWorkspace(double x, double y, double z, tf::StampedTransform *transform)
96 {
97  if (transform != NULL)
98  {
99  tf::Vector3 v(x, y, z);
100  tf::Vector3 tf_v = (*transform) * v;
101  x = tf_v.getX();
102  y = tf_v.getY();
103  z = tf_v.getZ();
104  }
105 
106  WorkspaceLimits limits = this->workspace_limits;
107 
108  if (x >= limits.min_x && x <= limits.max_x && y >= limits.min_y && y <= limits.max_y && z >= limits.min_z
109  && z <= limits.max_z)
110  {
111  return true;
112  }
113 
114  return false;
115 }
116 
117 PointCloud::Ptr Affordances::workspaceFilter(const PointCloud::Ptr &cloud_in, tf::StampedTransform *transform)
118 {
119  PointCloud::Ptr cloud_out(new PointCloud);
120 
121  for (std::size_t i = 0; i < cloud_in->points.size(); i++)
122  {
123  if (this->isPointInWorkspace(cloud_in->points[i].x, cloud_in->points[i].y, cloud_in->points[i].z, transform))
124  cloud_out->points.push_back(cloud_in->points[i]);
125  }
126 
127  return cloud_out;
128 }
129 
130 
131 int Affordances::numInFront(const PointCloud::Ptr &cloud, int center_index, double radius)
132 {
133  Eigen::Vector3f center = cloud->points[center_index].getVector3fMap();
134  double dist_center = center.norm();
135  double theta = atan(radius / dist_center);
136  Eigen::Vector3f center_unit = center / dist_center;
137  int num_in_front = 0;
138 
139  for (std::size_t i = 0; i < cloud->points.size(); i++)
140  {
141  if (isnan(cloud->points[i].x))
142  continue;
143 
144  Eigen::Vector3f point = cloud->points[i].getVector3fMap();
145  float point_norm = point.norm();
146  Eigen::Vector3f point_unit = point / point_norm;
147 
148  if (fabs(point_unit.dot(center_unit)) < cos(theta))
149  continue;
150 
151  if (point_norm < dist_center - radius)
152  num_in_front++;
153  }
154 
155  return num_in_front;
156 }
157 
158 void Affordances::estimateCurvatureAxisPCA(const PointCloud::Ptr &cloud, int nn_center_idx, std::vector<int> nn_indices,
159  Eigen::Vector3d &axis, Eigen::Vector3d &normal)
160 {
161  Eigen::Matrix3f covar_mat;
162  Eigen::Vector4f nn_centroid;
163  nn_centroid << cloud->points[nn_center_idx].x, cloud->points[nn_center_idx].y, cloud->points[nn_center_idx].z, 0;
164 
165  pcl::computeCovarianceMatrix(*cloud, nn_indices, nn_centroid, covar_mat);
166 
167  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covar_mat);
168  Eigen::Vector3f eig_vals = eigen_solver.eigenvalues();
169  int max_index;
170  eig_vals.maxCoeff(&max_index);
171  axis = eigen_solver.eigenvectors().col(max_index).cast<double>();
172  Eigen::Vector3d perp_axis;
173  perp_axis << -axis(1), axis(0), 0;
174  normal = axis.cross(perp_axis);
175  normal /= normal.norm();
176 }
177 
178 void Affordances::estimateCurvatureAxisNormals(const pcl::PointCloud<pcl::Normal>::Ptr &cloud_normals,
179  const std::vector<int> &nn_indices, Eigen::Vector3d &axis,
180  Eigen::Vector3d &normal)
181 {
182  Eigen::Matrix3d mat;
183  mat.setZero();
184 
185  for (std::size_t j = 0; j < nn_indices.size(); j++)
186  {
187  Eigen::Vector3d normal;
188  normal << cloud_normals->points[nn_indices[j]].normal[0], cloud_normals->points[nn_indices[j]].normal[1], cloud_normals->points[nn_indices[j]].normal[2];
189  Eigen::Matrix3d matNormal = normal * normal.transpose();
190  mat += matNormal;
191  }
192 
193  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(mat);
194  Eigen::Vector3d eig_vals = eigen_solver.eigenvalues();
195  int min_index;
196  eig_vals.minCoeff(&min_index);
197  axis = eigen_solver.eigenvectors().col(min_index);
198  Eigen::Vector3d perp_axis;
199  perp_axis << -axis(1), axis(0), 0;
200  normal = axis.cross(perp_axis);
201  normal /= normal.norm();
202 }
203 
204 void Affordances::estimateNormals(const PointCloud::Ptr &cloud, const pcl::PointCloud<pcl::Normal>::Ptr &cloud_normals)
205 {
206  pcl::NormalEstimationOMP<pcl::PointXYZRGBA, pcl::Normal> normal_estimator;
207  normal_estimator.setInputCloud(cloud);
208  pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGBA>());
209  normal_estimator.setSearchMethod(tree);
210  normal_estimator.setRadiusSearch(0.03);
211  normal_estimator.compute(*cloud_normals);
212 }
213 
214 std::vector<CylindricalShell> Affordances::searchAffordances(const PointCloud::Ptr &cloud,
215  tf::StampedTransform *transform)
216 {
217  std::vector<CylindricalShell> shells;
218  shells.resize(0);
219 
220  if (this->curvature_estimator == TAUBIN)
221  shells = this->searchAffordancesTaubin(cloud, transform);
222  else if (this->curvature_estimator == NORMALS)
223  shells = this->searchAffordancesNormalsOrPCA(cloud, transform);
224  else if (this->curvature_estimator == PCA)
225  shells = this->searchAffordancesNormalsOrPCA(cloud, transform);
226 
227  return shells;
228 }
229 
230 std::vector<CylindricalShell> Affordances::searchAffordancesNormalsOrPCA(const PointCloud::Ptr &cloud,
231  tf::StampedTransform *transform)
232 {
233  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
234 
235  // estimate surface normals
236  if (this->curvature_estimator == NORMALS)
237  {
238  double begin_time_normals_estimation = omp_get_wtime();
239  printf("Estimating surface normals ...\n");
240  this->estimateNormals(cloud, cloud_normals);
241  printf(" elapsed time: %.3f sec\n", omp_get_wtime() - begin_time_normals_estimation);
242  }
243 
244  // search cloud for a set of point neighborhoods
245  double begin_time_axis = omp_get_wtime();
246  printf("Estimating cylinder surface normal and curvature axis ...\n");
247  pcl::PointXYZRGBA searchPoint;
248  std::vector<int> nn_indices;
249  std::vector < std::vector<int> > neighborhoods(this->num_samples);
250  std::vector<int> neighborhood_centroids(this->num_samples);
251  std::vector<Eigen::Vector3d> normals(this->num_samples);
252  std::vector<Eigen::Vector3d> curvature_axes(this->num_samples);
253 
254  // set-up an Organized Neighbor search
255  if (cloud->isOrganized())
256  {
257  std::vector<float> nn_dists;
258  pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>::Ptr organized_neighbor(
259  new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>());
260  organized_neighbor->setInputCloud(cloud);
261  std::srand(std::time(0)); // use current time as seed for random generator
262 
263  for (int i = 0; i < this->num_samples; i++)
264  {
265  // sample random point from the point cloud
266  int r = std::rand() % cloud->points.size();
267 
268  while (!pcl::isFinite((*cloud)[r]) || !this->isPointInWorkspace((*cloud)[r].x, (*cloud)[r].y, (*cloud)[r].z))
269  r = std::rand() % cloud->points.size();
270 
271  // estimate cylinder curvature axis and normal
272  if (organized_neighbor->radiusSearch((*cloud)[r], this->NEIGHBOR_RADIUS, nn_indices, nn_dists) > 0)
273  {
274  if (this->curvature_estimator == NORMALS)
275  this->estimateCurvatureAxisNormals(cloud_normals, nn_indices, curvature_axes[i], normals[i]);
276  else if (this->curvature_estimator == PCA)
277  this->estimateCurvatureAxisPCA(cloud, r, nn_indices, curvature_axes[i], normals[i]);
278 
279  neighborhoods[i] = nn_indices;
280  neighborhood_centroids[i] = r;
281  }
282  }
283  }
284  else
285  {
286  std::vector<float> nn_dists;
287  pcl::KdTreeFLANN<pcl::PointXYZRGBA> tree;
288  tree.setInputCloud(cloud);
289  std::srand(std::time(0)); // use current time as seed for random generator
290 
291  for (int i = 0; i < this->num_samples; i++)
292  {
293  // sample random point from the point cloud
294  int r = std::rand() % cloud->points.size();
295 
296  while (!pcl::isFinite((*cloud)[r]) || !this->isPointInWorkspace((*cloud)[r].x, (*cloud)[r].y, (*cloud)[r].z))
297  r = std::rand() % cloud->points.size();
298 
299  // estimate cylinder curvature axis and normal
300  if (tree.radiusSearch((*cloud)[r], this->NEIGHBOR_RADIUS, nn_indices, nn_dists) > 0)
301  {
302  if (this->curvature_estimator == NORMALS)
303  this->estimateCurvatureAxisNormals(cloud_normals, nn_indices, curvature_axes[i], normals[i]);
304  else if (this->curvature_estimator == PCA)
305  this->estimateCurvatureAxisPCA(cloud, r, nn_indices, curvature_axes[i], normals[i]);
306 
307  neighborhoods[i] = nn_indices;
308  neighborhood_centroids[i] = r;
309  }
310  }
311  }
312 
313  printf(" elapsed time: %.3f sec\n", omp_get_wtime() - begin_time_axis);
314 
315  // define lower and upper bounds on radius of cylinder
316  double min_radius_cylinder = this->target_radius - this->radius_error;
317  double max_radius_cylinder = this->target_radius + this->radius_error;
318 
319  if (this->use_clearance_filter)
320  printf("Filtering on curvature, fitting cylinders, and filtering on low clearance ...\n");
321  else
322  printf("Filtering on curvature and fitting cylinders ...\n");
323 
324  double begin_time = omp_get_wtime();
325  int cylinders_left_clearance = 0;
326  Eigen::Vector3d normal;
327  Eigen::Vector3d curvature_axis;
328  Eigen::Vector3d curvature_centroid;
329  std::vector<CylindricalShell> shells;
330 
331  double maxHandAperture = this->target_radius + this->radius_error;
332  double outer_sample_radius = 1.5 * (maxHandAperture + this->handle_gap); // outer sample radius
333 
334  // for organized point clouds
335  pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>::Ptr organized_neighbor(
336  new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>());
337  // for unorganized point clouds
338  pcl::KdTreeFLANN<pcl::PointXYZRGBA> tree;
339  if (cloud->isOrganized())
340  organized_neighbor->setInputCloud(cloud);
341  else
342  tree.setInputCloud(cloud);
343 
344  for (int i = 0; i < this->num_samples; i++)
345  {
346  // fit a cylinder to the neighborhood
347  CylindricalShell shell;
348  shell.fitCylinder(cloud, neighborhoods[i], normals[i], curvature_axes[i]);
349 
350  // set height of shell to 2 * <target_radius>
351  shell.setExtent(2.0 * this->target_radius);
352 
353  // set index of centroid of neighborhood associated with the cylindrical shell
354  shell.setNeighborhoodCentroidIndex(neighborhood_centroids[i]);
355 
356  // check cylinder radius against target radius
357  if (shell.getRadius() > min_radius_cylinder && shell.getRadius() < max_radius_cylinder)
358  {
359  // filter on low clearance
360  if (this->use_clearance_filter)
361  {
362  pcl::PointXYZRGBA searchPoint;
363  std::vector<int> nn_indices;
364  std::vector<float> nn_dists;
365  Eigen::Vector3d centroid = shell.getCentroid();
366  searchPoint.x = centroid(0);
367  searchPoint.y = centroid(1);
368  searchPoint.z = centroid(2);
369  int num_in_radius = 0;
370  if (cloud->isOrganized())
371  num_in_radius = organized_neighbor->radiusSearch(searchPoint, outer_sample_radius, nn_indices, nn_dists);
372  else
373  num_in_radius = tree.radiusSearch(searchPoint, outer_sample_radius, nn_indices, nn_dists);
374 
375  if ((num_in_radius > 0) && (shell.hasClearance(cloud, nn_indices, maxHandAperture, this->handle_gap)))
376  shells.push_back(shell);
377  }
378  else
379  shells.push_back(shell);
380  }
381  }
382 
383  printf(" elapsed time: %.3f sec\n", omp_get_wtime() - begin_time);
384  if (this->use_clearance_filter)
385  printf(" cylinders left after clearance filtering: %i\n", (int)shells.size());
386 
387  return shells;
388 }
389 
390 std::vector<CylindricalShell> Affordances::searchAffordancesTaubin(const PointCloud::Ptr &cloud,
391  tf::StampedTransform *transform)
392 {
393  printf("Estimating curvature ...\n");
394  double beginTime = omp_get_wtime();
395 
396  // set-up estimator
398 
399  // set input source
400  estimator.setInputCloud(cloud);
401 
402  // set radius search
403  estimator.setRadiusSearch(this->NEIGHBOR_RADIUS);
404 
405  // set the number of samples
406  estimator.setNumSamples(this->num_samples);
407 
408  // provide a set of neighborhood centroids
409  std::vector<int> indices(this->num_samples);
410  std::srand(std::time(0)); // use current time as seed for random generator
411  int k;
412  for (int i = 0; i < this->num_samples; i++)
413  {
414  int r = std::rand() % cloud->points.size();
415  k = 0;
416  while (!pcl::isFinite((*cloud)[r])
417  || !this->isPointInWorkspace(cloud->points[r].x, cloud->points[r].y, cloud->points[r].z, transform))
418  {
419  r = std::rand() % cloud->points.size();
420  k++;
421  if (k == cloud->points.size()) // check that the cloud has finite points
422  {
423  printf("No finite points in cloud!\n");
424  std::vector<CylindricalShell> shells;
425  shells.resize(0);
426  return shells;
427  }
428  }
429  indices[i] = r;
430  }
431  boost::shared_ptr<std::vector<int> > indices_ptr(new std::vector<int>(indices));
432  estimator.setIndices(indices_ptr);
433 
434  // set number of threads
435  estimator.setNumThreads(this->num_threads);
436 
437  // output dataset
438  pcl::PointCloud<pcl::PointCurvatureTaubin>::Ptr cloud_curvature(new pcl::PointCloud<pcl::PointCurvatureTaubin>);
439 
440  // compute median curvature, normal axis, curvature axis, and curvature centroid
441  estimator.compute(*cloud_curvature);
442 
443  printf(" elapsed time: %.3f sec\n", omp_get_wtime() - beginTime);
444 
445  // define lower and upper bounds on radius of osculating sphere and cylinder
446  double min_radius_osculating_sphere = this->target_radius - 2.0 * this->radius_error;
447  double max_radius_osculating_sphere = this->target_radius + 2.0 * this->radius_error;
448  double min_radius_cylinder = this->target_radius - this->radius_error;
449  double max_radius_cylinder = this->target_radius + this->radius_error;
450 
451  if (this->use_clearance_filter)
452  printf("Filtering on curvature, fitting cylinders, and filtering on low clearance ...\n");
453  else
454  printf("Filtering on curvature and fitting cylinders ...\n");
455 
456  double begin_time = omp_get_wtime();
457  int cylinders_left_radius = 0;
458  int cylinders_left_clearance = 0;
459  Eigen::Vector3d normal;
460  Eigen::Vector3d curvature_axis;
461  std::vector<CylindricalShell> shells;
462 
463  double maxHandAperture = this->target_radius + this->radius_error;
464  double outer_sample_radius = 1.5 * (maxHandAperture + this->handle_gap); // outer sample radius
465 
466  // for organized point clouds
467  pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>::Ptr organized_neighbor(
468  new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>());
469  // for unorganized point clouds
470  pcl::KdTreeFLANN<pcl::PointXYZRGBA> tree;
471  if (cloud->isOrganized())
472  organized_neighbor->setInputCloud(cloud);
473  else
474  tree.setInputCloud(cloud);
475 
476  for (int i = 0; i < cloud_curvature->size(); i++)
477  {
478  // calculate radius of osculating sphere
479  double radius = 1.0 / fabs(cloud_curvature->points[i].median_curvature);
480 
481  // filter out planar regions and cylinders that are too large
482  if (radius > min_radius_osculating_sphere && radius < max_radius_osculating_sphere)
483  {
484  // fit a cylinder to the neighborhood
485  normal << cloud_curvature->points[i].normal_x, cloud_curvature->points[i].normal_y, cloud_curvature->points[i].normal_z;
486  curvature_axis << cloud_curvature->points[i].curvature_axis_x, cloud_curvature->points[i].curvature_axis_y, cloud_curvature->points[i].curvature_axis_z;
487  CylindricalShell shell;
488  shell.fitCylinder(cloud, estimator.getNeighborhoods()[i], normal, curvature_axis);
489 
490  // set height of shell to 2 * <target_radius>
491  shell.setExtent(2.0 * this->target_radius);
492 
493  // set index of centroid of neighborhood associated with the cylindrical shell
495 
496  // check cylinder radius against target radius
497  if (shell.getRadius() > min_radius_cylinder && shell.getRadius() < max_radius_cylinder)
498  {
499  cylinders_left_radius++;
500 
501  // filter on low clearance
502  if (this->use_clearance_filter)
503  {
504  pcl::PointXYZRGBA searchPoint;
505  std::vector<int> nn_indices;
506  std::vector<float> nn_dists;
507  Eigen::Vector3d centroid = shell.getCentroid();
508  searchPoint.x = centroid(0);
509  searchPoint.y = centroid(1);
510  searchPoint.z = centroid(2);
511  int num_in_radius = 0;
512  if (cloud->isOrganized())
513  num_in_radius = organized_neighbor->radiusSearch(searchPoint, outer_sample_radius, nn_indices, nn_dists);
514  else
515  num_in_radius = tree.radiusSearch(searchPoint, outer_sample_radius, nn_indices, nn_dists);
516 
517  if ((num_in_radius > 0) && (shell.hasClearance(cloud, nn_indices, maxHandAperture, this->handle_gap)))
518  shells.push_back(shell);
519  }
520  else
521  shells.push_back(shell);
522  }
523  }
524  }
525 
526  printf(" elapsed time: %.3f sec\n", omp_get_wtime() - begin_time);
527  printf(" cylinders left after radius filtering: %i\n", cylinders_left_radius);
528  if (this->use_clearance_filter)
529  printf(" cylinders left after clearance filtering: %i\n", (int)shells.size());
530 
531  return shells;
532 }
533 
534 std::vector<std::vector<CylindricalShell> > Affordances::searchHandles(const PointCloud::Ptr &cloud,
535  std::vector<CylindricalShell> shells)
536 {
537  std::vector < std::vector<CylindricalShell> > handles;
538 
539  // find colinear sets of cylinders
540  if (this->alignment_runs > 0)
541  {
542  std::cout << "alignment search for colinear sets of cylinders (handles) ... " << std::endl;
543  double beginTime = omp_get_wtime();
544  std::vector<int> inliersMaxSet, outliersMaxSet;
545 
546  // linear search
547  for (int i = 0; i < this->alignment_runs && shells.size() > 0; i++)
548  {
549  this->findBestColinearSet(shells, inliersMaxSet, outliersMaxSet);
550  printf(" number of inliers in run %i: %i", i, (int)inliersMaxSet.size());
551 
552  if (inliersMaxSet.size() >= this->alignment_min_inliers)
553  {
554  // create handle from inlier indices
555  std::vector<CylindricalShell> handle;
556  for (std::size_t j = 0; j < inliersMaxSet.size(); j++)
557  {
558  int idx = inliersMaxSet[j];
559  handle.push_back(shells[idx]);
560  }
561 
562  // check for occlusions
563  if (this->use_occlusion_filter)
564  {
565  int MAX_NUM_OCCLUDED = (int)handle.size() * 0.5; // 5
566  int num_occluded = 0;
567  bool is_occluded = false;
568 
569  for (std::size_t j = 0; j < handle.size(); j++)
570  {
571  if (this->numInFront(cloud, handle[j].getNeighborhoodCentroidIndex(),
572  1.5 * this->target_radius + this->radius_error) > this->MAX_NUM_IN_FRONT)
573  {
574  num_occluded++;
575  if (num_occluded > MAX_NUM_OCCLUDED)
576  {
577  is_occluded = true;
578  break;
579  }
580  }
581  }
582 
583  printf(" number of occluded affordances: %i; occluded: %s\n", num_occluded, is_occluded ? "true" : "false");
584 
585  if (!is_occluded)
586  handles.push_back(handle);
587  }
588  else
589  {
590  handles.push_back(handle);
591  }
592 
593  // prune list of cylindrical shells
594  std::vector<CylindricalShell> remainder(outliersMaxSet.size());
595  for (std::size_t j = 0; j < outliersMaxSet.size(); j++)
596  remainder[j] = shells[outliersMaxSet[j]];
597  shells = remainder;
598  printf(", remaining cylinders: %i\n", (int)shells.size());
599  }
600  // do not check for occlusions
601  else
602  {
603  break;
604  }
605  }
606 
607  printf(" elapsed time: %.3f\n", omp_get_wtime() - beginTime);
608  }
609 
610  return handles;
611 }
612 
613 std::vector<CylindricalShell> Affordances::searchAffordances(const PointCloud::Ptr &cloud,
614  const std::vector<int> &indices)
615 {
616  Eigen::MatrixXd samples(3, indices.size());
617  for (std::size_t i = 0; i < indices.size(); i++)
618  samples.col(i) = cloud->points[indices[i]].getVector3fMap().cast<double>();
619 
620  return this->searchAffordancesTaubin(cloud, samples);
621 }
622 
623 std::vector<CylindricalShell> Affordances::searchAffordancesTaubin(const PointCloud::Ptr &cloud,
624  const Eigen::MatrixXd &samples, bool is_logging)
625 {
626  if (is_logging)
627  printf("Estimating curvature ...\n");
628 
629  double beginTime = omp_get_wtime();
630 
631  // set-up estimator
632  pcl::PointCloud<pcl::PointCurvatureTaubin>::Ptr cloud_curvature(new pcl::PointCloud<pcl::PointCurvatureTaubin>);
634  estimator.setInputCloud(cloud);
635  estimator.setRadiusSearch(this->NEIGHBOR_RADIUS);
636  //~ estimator.setRadiusSearch(1.5*target_radius + radius_error);
637  estimator.setNumThreads(this->num_threads);
638 
639  // compute median curvature, normal axis, curvature axis, and curvature centroid
640  estimator.computeFeature(samples, *cloud_curvature);
641 
642  if (is_logging)
643  printf(" elapsed time: %.3f sec, cylinders left: %i\n", omp_get_wtime() - beginTime,
644  (int)cloud_curvature->points.size());
645 
646  // define lower and upper bounds on radius of osculating sphere and cylinder
647  double min_radius_osculating_sphere = this->target_radius - 2.0 * this->radius_error;
648  double max_radius_osculating_sphere = this->target_radius + 2.0 * this->radius_error;
649  double min_radius_cylinder = this->target_radius - this->radius_error;
650  double max_radius_cylinder = this->target_radius + this->radius_error;
651 
652  if (is_logging && this->use_clearance_filter)
653  printf("Filtering on curvature, fitting cylinders, and filtering on low clearance ...\n");
654  else if (is_logging)
655  printf("Filtering on curvature and fitting cylinders ...\n");
656 
657  double begin_time = omp_get_wtime();
658  int cylinders_left_radius = 0;
659  int cylinders_left_clearance = 0;
660  Eigen::Vector3d normal;
661  Eigen::Vector3d curvature_axis;
662  std::vector<CylindricalShell> shells;
663  double tcyltotal = 0.0;
664  double tcleartotal = 0.0;
665 
666  double maxHandAperture = this->target_radius + this->radius_error;
667  double outer_sample_radius = 1.5 * (maxHandAperture + this->handle_gap); // outer sample radius
668 
669  // for organized point clouds
670  pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>::Ptr organized_neighbor(
671  new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>());
672  // for unorganized point clouds
673  pcl::KdTreeFLANN<pcl::PointXYZRGBA> tree;
674  if (cloud->isOrganized())
675  organized_neighbor->setInputCloud(cloud);
676  else
677  tree.setInputCloud(cloud);
678 
679  //~ #ifdef _OPENMP
680  //~ #pragma omp parallel for shared (cylinderList) firstprivate(cloud_curvature) private(radius, centroid_cyl, extent_cyl, normal, curvature_axis, curvature_centroid) num_threads(this->num_threads)
681  //~ #endif
682  for (int i = 0; i < cloud_curvature->size(); i++)
683  {
684  if (isnan(cloud_curvature->points[i].normal[0]))
685  continue;
686 
687  // calculate radius of osculating sphere
688  double radius = 1.0 / fabs(cloud_curvature->points[i].median_curvature);
689  //~ printf("%i: radius of osculating sphere: %.4f\n", i, radius);
690 
691  // filter out planar regions and cylinders that are too large
692  if (radius > min_radius_osculating_sphere && radius < max_radius_osculating_sphere)
693  {
694  // fit a cylinder to the neighborhood
695  double tcyl0 = omp_get_wtime();
696  normal << cloud_curvature->points[i].normal_x, cloud_curvature->points[i].normal_y, cloud_curvature->points[i].normal_z;
697  curvature_axis << cloud_curvature->points[i].curvature_axis_x, cloud_curvature->points[i].curvature_axis_y, cloud_curvature->points[i].curvature_axis_z;
698  CylindricalShell shell;
699  shell.fitCylinder(cloud, estimator.getNeighborhoods()[i], normal, curvature_axis);
700  tcyltotal += omp_get_wtime() - tcyl0;
701 
702  //~ printf(" radius of fitted cylinder: %.4f\n", shell.getRadius());
703 
704  // set height of shell to 2 * <target_radius>
705  shell.setExtent(2.0 * this->target_radius);
706 
707  // set index of centroid of neighborhood associated with the cylindrical shell
709 
710  // check cylinder radius against target radius
711  if (shell.getRadius() > min_radius_cylinder && shell.getRadius() < max_radius_cylinder)
712  {
713  cylinders_left_radius++;
714 
715  // filter on low clearance
716  if (this->use_clearance_filter)
717  {
718  double tclear0 = omp_get_wtime();
719  pcl::PointXYZRGBA searchPoint;
720  std::vector<int> nn_indices;
721  std::vector<float> nn_dists;
722  Eigen::Vector3d centroid = shell.getCentroid();
723  searchPoint.x = centroid(0);
724  searchPoint.y = centroid(1);
725  searchPoint.z = centroid(2);
726  int num_in_radius = 0;
727  if (cloud->isOrganized())
728  num_in_radius = organized_neighbor->radiusSearch(searchPoint, outer_sample_radius, nn_indices, nn_dists);
729  else
730  num_in_radius = tree.radiusSearch(searchPoint, outer_sample_radius, nn_indices, nn_dists);
731 
732  if ((num_in_radius > 0) && (shell.hasClearance(cloud, nn_indices, maxHandAperture, this->handle_gap)))
733  shells.push_back(shell);
734 
735  tcleartotal += omp_get_wtime() - tclear0;
736  }
737  else
738  shells.push_back(shell);
739  }
740  }
741  }
742 
743  if (is_logging)
744  {
745  printf(" elapsed time: %.3f sec\n", omp_get_wtime() - begin_time);
746  printf(" cylinders left after radius filtering: %i\n", cylinders_left_radius);
747  if (this->use_clearance_filter)
748  printf(" cylinders left after clearance filtering: %i\n", (int)shells.size());
749  printf(" cylinder/circle fitting: %.3f sec\n", tcyltotal);
750  printf(" shell search: %.3f sec\n", tcleartotal);
751  }
752 
753  return shells;
754 }
755 
756 std::vector<int> Affordances::createRandomIndices(const PointCloud::Ptr &cloud, int size)
757 {
758  std::vector<int> indices(size);
759 
760  for (int i = 0; i < size; i++)
761  {
762  int r = std::rand() % cloud->points.size();
763  while (!pcl::isFinite((*cloud)[r])
764  || !this->isPointInWorkspace(cloud->points[r].x, cloud->points[r].y, cloud->points[r].z))
765  r = std::rand() % cloud->points.size();
766  indices[i] = r;
767  }
768 
769  return indices;
770 }
771 
772 void Affordances::findBestColinearSet(const std::vector<CylindricalShell> &list, std::vector<int> &inliersMaxSet,
773  std::vector<int> &outliersMaxSet)
774 {
775  int maxInliers = 0;
776  double orientRadius2 = this->alignment_orient_radius * this->alignment_orient_radius;
777  double distRadius2 = this->alignment_dist_radius * this->alignment_dist_radius;
778 
779  for (std::size_t i = 0; i < list.size(); i++)
780  {
781  Eigen::Vector3d axis = list[i].getCurvatureAxis();
782  Eigen::Vector3d centroid = list[i].getCentroid();
783  double radius = list[i].getRadius();
784  std::vector<int> inliers, outliers;
785 
786  for (std::size_t j = 0; j < list.size(); j++)
787  {
788  Eigen::Vector3d distToOrientVec = (Eigen::MatrixXd::Identity(3, 3) - axis * axis.transpose())
789  * list[j].getCurvatureAxis();
790  double distToOrient = distToOrientVec.cwiseProduct(distToOrientVec).sum();
791  Eigen::Vector3d distToAxisVec = (Eigen::MatrixXd::Identity(3, 3) - axis * axis.transpose())
792  * (list[j].getCentroid() - centroid);
793  double distToAxis = distToAxisVec.cwiseProduct(distToAxisVec).sum();
794  double distToRadius = fabs(list[j].getRadius() - radius);
795 
796  if (distToOrient < orientRadius2 && distToAxis < distRadius2 && distToRadius < this->alignment_radius_radius)
797  inliers.push_back(j);
798  else
799  outliers.push_back(j);
800  }
801 
802  if (inliers.size() > maxInliers)
803  {
804  maxInliers = inliers.size();
805  inliersMaxSet = inliers;
806  outliersMaxSet = outliers;
807  }
808  }
809 }
static const double ALIGNMENT_RADIUS_RADIUS
Definition: affordances.h:256
const std::string CURVATURE_ESTIMATORS[]
Definition: affordances.cpp:9
static const double RADIUS_ERROR
Definition: affordances.h:247
double handle_gap
Definition: affordances.h:225
void initParams(ros::NodeHandle node)
Read the parameters from a ROS launch file.
Definition: affordances.cpp:30
static const double WORKSPACE_MIN
Definition: affordances.h:257
CylindricalShell represents a cylindrical shell that consists of two colinear cylinders. A shell consists of an inner and an outer cylinder. The portion of the object to be grasped must fit inside the inner cylinder, and the radius of that cylinder must be no larger than the maximum hand aperture. The gap between the inner and outer cylinder must be free of obstacles and wide enough to be able to contain the robot fingers.
double target_radius
Definition: affordances.h:223
int alignment_min_inliers
Definition: affordances.h:232
PointCloud::Ptr maxRangeFilter(const PointCloud::Ptr &cloud_in)
Filter out all points from a given cloud that are outside of a sphere with a radius max_range and cen...
Definition: affordances.cpp:81
std::vector< std::vector< int > > const & getNeighborhoods() const
Get the indices of each point neighborhood.
double radius_error
Definition: affordances.h:224
WorkspaceLimits workspace_limits
Definition: affordances.h:236
void estimateNormals(const PointCloud::Ptr &cloud, const pcl::PointCloud< pcl::Normal >::Ptr &cloud_normals)
Estimate surface normals for each point in the point cloud.
static const double ALIGNMENT_DIST_RADIUS
Definition: affordances.h:254
CurvatureEstimationTaubin estimates the curvature for a set of point neighborhoods in the cloud using...
bool use_occlusion_filter
Definition: affordances.h:229
TFSIMD_FORCE_INLINE const tfScalar & getY() const
static const int ALIGNMENT_RUNS
Definition: affordances.h:252
void setExtent(double extent)
Set the extent of the cylindrical shell.
TFSIMD_FORCE_INLINE const tfScalar & y() const
std::string file
Definition: affordances.h:238
std::vector< CylindricalShell > searchAffordancesNormalsOrPCA(const PointCloud::Ptr &cloud, tf::StampedTransform *transform=NULL)
Search grasp affordances (cylindrical shells) in a given point cloud using surface normals...
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
std::vector< CylindricalShell > searchAffordances(const PointCloud::Ptr &cloud, tf::StampedTransform *transform=NULL)
Search grasp affordances (cylindrical shells) in a given point cloud.
static const double TARGET_RADIUS
Definition: affordances.h:246
int numInFront(const PointCloud::Ptr &cloud, int center_index, double radius)
Find the number of points in the point cloud that lie in front of a point neighborhood with a given c...
PointCloud::Ptr workspaceFilter(const PointCloud::Ptr &cloud_in, tf::StampedTransform *transform=NULL)
Filter out all points from a given cloud that are outside of a cube defined by the workspace limits o...
void computeFeature(const Eigen::MatrixXd &samples, PointCloudOut &output)
Estimate the curvature for a set of point neighborhoods with given centroids.
static const double NEIGHBOR_RADIUS
Definition: affordances.h:244
void setNumSamples(int num_samples)
Set the number of samples (point neighborhoods).
std::vector< int > createRandomIndices(const PointCloud::Ptr &cloud, int size)
int curvature_estimator
Definition: affordances.h:230
static const bool USE_CLEARANCE_FILTER
Definition: affordances.h:250
void fitCylinder(const PointCloud::Ptr &cloud, const std::vector< int > &indices, const Eigen::Vector3d &normal, const Eigen::Vector3d &curvature_axis)
Fit a cylinder to a set of points in the cloud, using their indices, and the normal and the curvature...
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
bool use_clearance_filter
Definition: affordances.h:228
bool hasClearance(const PointCloud::Ptr &cloud, const std::vector< int > &nn_indices, double maxHandAperture, double handleGap)
Check whether the gap between the inner and outer cylinder of the shell is free of obstacles and wide...
void estimateCurvatureAxisNormals(const pcl::PointCloud< pcl::Normal >::Ptr &cloud_normals, const std::vector< int > &nn_indices, Eigen::Vector3d &axis, Eigen::Vector3d &normal)
Estimate the cylinder&#39;s curvature axis and normal using surface normals.
pcl::PointCloud< pcl::PointXYZRGBA > PointCloud
Definition: affordances.h:53
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void findBestColinearSet(const std::vector< CylindricalShell > &list, std::vector< int > &inliersMaxSet, std::vector< int > &outliersMaxSet)
Find the best (largest number of inliers) set of colinear cylindrical shells given a list of shells...
TFSIMD_FORCE_INLINE const tfScalar & x() const
void estimateCurvatureAxisPCA(const PointCloud::Ptr &cloud, int nn_center_idx, std::vector< int > nn_indices, Eigen::Vector3d &axis, Eigen::Vector3d &normal)
Estimate the cylinder&#39;s curvature axis and normal using PCA.
std::vector< std::vector< CylindricalShell > > searchHandles(const PointCloud::Ptr &cloud, std::vector< CylindricalShell > shells)
Search handles in a set of cylindrical shells. If occlusion filtering is turned on (using the corresp...
pcl::PointCloud< pcl::PointXYZRGBA > PointCloud
Definition: affordances.cpp:3
double alignment_orient_radius
Definition: affordances.h:234
static const int ALIGNMENT_MIN_INLIERS
Definition: affordances.h:253
TFSIMD_FORCE_INLINE const tfScalar & z() const
Eigen::Vector3d getCentroid() const
Get the centroid of the cylindrical shell.
std::vector< int > const & getNeighborhoodCentroids() const
Get the centroid indices of each point neighborhood.
static const int NUM_NEAREST_NEIGHBORS
Definition: affordances.h:243
const int NORMALS
Definition: affordances.cpp:7
std::vector< CylindricalShell > searchAffordancesTaubin(const PointCloud::Ptr &cloud, const Eigen::MatrixXd &samples, bool is_logging=true)
Search grasp affordances (cylindrical shells) using a set of samples in a given point cloud...
static const bool USE_OCCLUSION_FILTER
Definition: affordances.h:251
double alignment_dist_radius
Definition: affordances.h:233
const int TAUBIN
Definition: affordances.cpp:5
TFSIMD_FORCE_INLINE const tfScalar & getX() const
static const double MAX_RANGE
Definition: affordances.h:249
double alignment_radius_radius
Definition: affordances.h:235
double getRadius() const
Get the radius of the cylindrical shell.
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
static const double WORKSPACE_MAX
Definition: affordances.h:258
static const double ALIGNMENT_ORIENT_RADIUS
Definition: affordances.h:255
static const double HANDLE_GAP
Definition: affordances.h:248
void setNumThreads(int num_threads)
Set the number of threads used for parallelizing Taubin Quadric Fitting.
void setNeighborhoodCentroidIndex(int index)
Set the index of the centroid of the neighborhood associated with the cylindrical shell...
const int PCA
Definition: affordances.cpp:6
static const int MAX_NUM_IN_FRONT
Definition: affordances.h:245
static const int CURVATURE_ESTIMATOR
Definition: affordances.h:241
int alignment_runs
Definition: affordances.h:231
static const int NUM_SAMPLES
Definition: affordances.h:242
bool isPointInWorkspace(double x, double y, double z, tf::StampedTransform *transform=NULL)
Check whether a given point, using its x, y, and z coordinates, is within the workspace of the robot...
Definition: affordances.cpp:95
double max_range
Definition: affordances.h:227


handle_detector
Author(s):
autogenerated on Mon Jun 10 2019 13:29:00