affordances.cpp
Go to the documentation of this file.
00001 #include <handle_detector/affordances.h>
00002 
00003 typedef pcl::PointCloud<pcl::PointXYZRGBA> PointCloud;
00004 
00005 const int TAUBIN = 0;
00006 const int PCA = 1;
00007 const int NORMALS = 2;
00008 
00009 const std::string CURVATURE_ESTIMATORS[] = {"Taubin", "PCA", "Normals"};
00010 
00011 const int Affordances::CURVATURE_ESTIMATOR = 0;
00012 const int Affordances::NUM_SAMPLES = 5000;
00013 const int Affordances::NUM_NEAREST_NEIGHBORS = 500;
00014 const double Affordances::NEIGHBOR_RADIUS = 0.025;
00015 const int Affordances::MAX_NUM_IN_FRONT = 20;
00016 const double Affordances::TARGET_RADIUS = 0.08;
00017 const double Affordances::RADIUS_ERROR = 0.013;
00018 const double Affordances::HANDLE_GAP = 0.08;
00019 const double Affordances::MAX_RANGE = 1.0;
00020 const bool Affordances::USE_CLEARANCE_FILTER = true;
00021 const bool Affordances::USE_OCCLUSION_FILTER = true;
00022 const int Affordances::ALIGNMENT_RUNS = 3;
00023 const int Affordances::ALIGNMENT_MIN_INLIERS = 10;
00024 const double Affordances::ALIGNMENT_DIST_RADIUS = 0.02;
00025 const double Affordances::ALIGNMENT_ORIENT_RADIUS = 0.1;
00026 const double Affordances::ALIGNMENT_RADIUS_RADIUS = 0.003;
00027 const double Affordances::WORKSPACE_MIN = -1.0;
00028 const double Affordances::WORKSPACE_MAX = 1.0;
00029 
00030 void Affordances::initParams(ros::NodeHandle node)
00031 {
00032   // read parameters from ROS launch file
00033   std::string file_default = "";
00034   node.param("file", this->file, file_default);
00035   node.param("target_radius", this->target_radius, this->TARGET_RADIUS);
00036   node.param("target_radius_error", this->radius_error, this->RADIUS_ERROR);
00037   node.param("affordance_gap", this->handle_gap, this->HANDLE_GAP);
00038   node.param("sample_size", this->num_samples, this->NUM_SAMPLES);
00039   node.param("max_range", this->max_range, this->MAX_RANGE);
00040   node.param("use_clearance_filter", this->use_clearance_filter, this->USE_CLEARANCE_FILTER);
00041   node.param("use_occlusion_filter", this->use_occlusion_filter, this->USE_OCCLUSION_FILTER);
00042   node.param("curvature_estimator", this->curvature_estimator, this->CURVATURE_ESTIMATOR);
00043   node.param("alignment_runs", this->alignment_runs, this->ALIGNMENT_RUNS);
00044   node.param("alignment_min_inliers", this->alignment_min_inliers, this->ALIGNMENT_MIN_INLIERS);
00045   node.param("alignment_dist_radius", this->alignment_dist_radius, this->ALIGNMENT_DIST_RADIUS);
00046   node.param("alignment_orient_radius", this->alignment_orient_radius, this->ALIGNMENT_ORIENT_RADIUS);
00047   node.param("alignment_radius_radius", this->alignment_radius_radius, this->ALIGNMENT_RADIUS_RADIUS);
00048   node.param("workspace_min_x", this->workspace_limits.min_x, this->WORKSPACE_MIN);
00049   node.param("workspace_max_x", this->workspace_limits.max_x, this->WORKSPACE_MAX);
00050   node.param("workspace_min_y", this->workspace_limits.min_y, this->WORKSPACE_MIN);
00051   node.param("workspace_max_y", this->workspace_limits.max_y, this->WORKSPACE_MAX);
00052   node.param("workspace_min_z", this->workspace_limits.min_z, this->WORKSPACE_MIN);
00053   node.param("workspace_max_z", this->workspace_limits.max_z, this->WORKSPACE_MAX);
00054   node.param("num_threads", this->num_threads, 1);
00055 
00056   // print parameters
00057   printf("PARAMETERS\n");
00058   printf(" file: %s\n", this->file.c_str());
00059   printf(" target radius: %.3f\n", this->target_radius);
00060   printf(" target radius error: %.3f\n", this->radius_error);
00061   printf(" min. affordance gap: %.3f\n", this->handle_gap);
00062   printf(" number of samples: %i\n", this->num_samples);
00063   printf(" max. range: %.3f\n", this->max_range);
00064   printf(" use clearance filter: %s\n", this->use_clearance_filter ? "true" : "false");
00065   printf(" use occlusion filter: %s\n", this->use_occlusion_filter ? "true" : "false");
00066   printf(" curvature estimator: %s\n", CURVATURE_ESTIMATORS[this->curvature_estimator].c_str());
00067   printf(" number of alignment runs: %i\n", this->alignment_runs);
00068   printf(" min. number of alignment inliers: %i\n", this->alignment_min_inliers);
00069   printf(" alignment distance threshold: %.3f\n", this->alignment_dist_radius);
00070   printf(" alignment orientation threshold: %.3f\n", this->alignment_orient_radius);
00071   printf(" alignment radius threshold: %.3f\n", this->alignment_radius_radius);
00072   printf(" workspace_min_x: %.3f\n", this->workspace_limits.min_x);
00073   printf(" workspace_max_x: %.3f\n", this->workspace_limits.max_x);
00074   printf(" workspace_min_y: %.3f\n", this->workspace_limits.min_y);
00075   printf(" workspace_max_y: %.3f\n", this->workspace_limits.max_y);
00076   printf(" workspace_min_z: %.3f\n", this->workspace_limits.min_z);
00077   printf(" workspace_max_z: %.3f\n", this->workspace_limits.max_z);
00078   printf(" num_threads: %i\n", this->num_threads);
00079 }
00080 
00081 PointCloud::Ptr Affordances::maxRangeFilter(const PointCloud::Ptr &cloud_in)
00082 {
00083   PointCloud::Ptr cloud_out(new PointCloud);
00084 
00085   for (std::size_t i = 0; i < cloud_in->points.size(); i++)
00086   {
00087     if (cloud_in->points[i].x * cloud_in->points[i].x + cloud_in->points[i].y * cloud_in->points[i].y
00088         + cloud_in->points[i].z * cloud_in->points[i].z < this->max_range * this->max_range)
00089       cloud_out->points.push_back(cloud_in->points[i]);
00090   }
00091 
00092   return cloud_out;
00093 }
00094 
00095 bool Affordances::isPointInWorkspace(double x, double y, double z, tf::StampedTransform *transform)
00096 {
00097   if (transform != NULL)
00098   {
00099     tf::Vector3 v(x, y, z);
00100     tf::Vector3 tf_v = (*transform) * v;
00101     x = tf_v.getX();
00102     y = tf_v.getY();
00103     z = tf_v.getZ();
00104   }
00105 
00106   WorkspaceLimits limits = this->workspace_limits;
00107 
00108   if (x >= limits.min_x && x <= limits.max_x && y >= limits.min_y && y <= limits.max_y && z >= limits.min_z
00109       && z <= limits.max_z)
00110   {
00111     return true;
00112   }
00113 
00114   return false;
00115 }
00116 
00117 PointCloud::Ptr Affordances::workspaceFilter(const PointCloud::Ptr &cloud_in, tf::StampedTransform *transform)
00118 {
00119   PointCloud::Ptr cloud_out(new PointCloud);
00120 
00121   for (std::size_t i = 0; i < cloud_in->points.size(); i++)
00122   {
00123     if (this->isPointInWorkspace(cloud_in->points[i].x, cloud_in->points[i].y, cloud_in->points[i].z, transform))
00124       cloud_out->points.push_back(cloud_in->points[i]);
00125   }
00126 
00127   return cloud_out;
00128 }
00129 
00130 
00131 int Affordances::numInFront(const PointCloud::Ptr &cloud, int center_index, double radius)
00132 {
00133   Eigen::Vector3f center = cloud->points[center_index].getVector3fMap();
00134   double dist_center = center.norm();
00135   double theta = atan(radius / dist_center);
00136   Eigen::Vector3f center_unit = center / dist_center;
00137   int num_in_front = 0;
00138 
00139   for (std::size_t i = 0; i < cloud->points.size(); i++)
00140   {
00141     if (isnan(cloud->points[i].x))
00142       continue;
00143 
00144     Eigen::Vector3f point = cloud->points[i].getVector3fMap();
00145     float point_norm = point.norm();
00146     Eigen::Vector3f point_unit = point / point_norm;
00147 
00148     if (fabs(point_unit.dot(center_unit)) < cos(theta))
00149       continue;
00150 
00151     if (point_norm < dist_center - radius)
00152       num_in_front++;
00153   }
00154 
00155   return num_in_front;
00156 }
00157 
00158 void Affordances::estimateCurvatureAxisPCA(const PointCloud::Ptr &cloud, int nn_center_idx, std::vector<int> nn_indices,
00159                                            Eigen::Vector3d &axis, Eigen::Vector3d &normal)
00160 {
00161   Eigen::Matrix3f covar_mat;
00162   Eigen::Vector4f nn_centroid;
00163   nn_centroid << cloud->points[nn_center_idx].x, cloud->points[nn_center_idx].y, cloud->points[nn_center_idx].z, 0;
00164 
00165   pcl::computeCovarianceMatrix(*cloud, nn_indices, nn_centroid, covar_mat);
00166 
00167   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covar_mat);
00168   Eigen::Vector3f eig_vals = eigen_solver.eigenvalues();
00169   int max_index;
00170   eig_vals.maxCoeff(&max_index);
00171   axis = eigen_solver.eigenvectors().col(max_index).cast<double>();
00172   Eigen::Vector3d perp_axis;
00173   perp_axis << -axis(1), axis(0), 0;
00174   normal = axis.cross(perp_axis);
00175   normal /= normal.norm();
00176 }
00177 
00178 void Affordances::estimateCurvatureAxisNormals(const pcl::PointCloud<pcl::Normal>::Ptr &cloud_normals,
00179                                                const std::vector<int> &nn_indices, Eigen::Vector3d &axis,
00180                                                Eigen::Vector3d &normal)
00181 {
00182   Eigen::Matrix3d mat;
00183   mat.setZero();
00184 
00185   for (std::size_t j = 0; j < nn_indices.size(); j++)
00186   {
00187     Eigen::Vector3d normal;
00188     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];
00189     Eigen::Matrix3d matNormal = normal * normal.transpose();
00190     mat += matNormal;
00191   }
00192 
00193   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(mat);
00194   Eigen::Vector3d eig_vals = eigen_solver.eigenvalues();
00195   int min_index;
00196   eig_vals.minCoeff(&min_index);
00197   axis = eigen_solver.eigenvectors().col(min_index);
00198   Eigen::Vector3d perp_axis;
00199   perp_axis << -axis(1), axis(0), 0;
00200   normal = axis.cross(perp_axis);
00201   normal /= normal.norm();
00202 }
00203 
00204 void Affordances::estimateNormals(const PointCloud::Ptr &cloud, const pcl::PointCloud<pcl::Normal>::Ptr &cloud_normals)
00205 {
00206   pcl::NormalEstimationOMP<pcl::PointXYZRGBA, pcl::Normal> normal_estimator;
00207   normal_estimator.setInputCloud(cloud);
00208   pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGBA>());
00209   normal_estimator.setSearchMethod(tree);
00210   normal_estimator.setRadiusSearch(0.03);
00211   normal_estimator.compute(*cloud_normals);
00212 }
00213 
00214 std::vector<CylindricalShell> Affordances::searchAffordances(const PointCloud::Ptr &cloud,
00215                                                              tf::StampedTransform *transform)
00216 {
00217   std::vector<CylindricalShell> shells;
00218   shells.resize(0);
00219 
00220   if (this->curvature_estimator == TAUBIN)
00221     shells = this->searchAffordancesTaubin(cloud, transform);
00222   else if (this->curvature_estimator == NORMALS)
00223     shells = this->searchAffordancesNormalsOrPCA(cloud, transform);
00224   else if (this->curvature_estimator == PCA)
00225     shells = this->searchAffordancesNormalsOrPCA(cloud, transform);
00226 
00227   return shells;
00228 }
00229 
00230 std::vector<CylindricalShell> Affordances::searchAffordancesNormalsOrPCA(const PointCloud::Ptr &cloud,
00231                                                                          tf::StampedTransform *transform)
00232 {
00233   pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
00234 
00235   // estimate surface normals
00236   if (this->curvature_estimator == NORMALS)
00237   {
00238     double begin_time_normals_estimation = omp_get_wtime();
00239     printf("Estimating surface normals ...\n");
00240     this->estimateNormals(cloud, cloud_normals);
00241     printf(" elapsed time: %.3f sec\n", omp_get_wtime() - begin_time_normals_estimation);
00242   }
00243 
00244   // search cloud for a set of point neighborhoods
00245   double begin_time_axis = omp_get_wtime();
00246   printf("Estimating cylinder surface normal and curvature axis ...\n");
00247   pcl::PointXYZRGBA searchPoint;
00248   std::vector<int> nn_indices;
00249   std::vector < std::vector<int> > neighborhoods(this->num_samples);
00250   std::vector<int> neighborhood_centroids(this->num_samples);
00251   std::vector<Eigen::Vector3d> normals(this->num_samples);
00252   std::vector<Eigen::Vector3d> curvature_axes(this->num_samples);
00253 
00254   // set-up an Organized Neighbor search
00255   if (cloud->isOrganized())
00256   {
00257     std::vector<float> nn_dists;
00258     pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>::Ptr organized_neighbor(
00259         new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>());
00260     organized_neighbor->setInputCloud(cloud);
00261     std::srand(std::time(0)); // use current time as seed for random generator
00262 
00263     for (int i = 0; i < this->num_samples; i++)
00264     {
00265       // sample random point from the point cloud
00266       int r = std::rand() % cloud->points.size();
00267 
00268       while (!pcl::isFinite((*cloud)[r]) || !this->isPointInWorkspace((*cloud)[r].x, (*cloud)[r].y, (*cloud)[r].z))
00269         r = std::rand() % cloud->points.size();
00270 
00271       // estimate cylinder curvature axis and normal
00272       if (organized_neighbor->radiusSearch((*cloud)[r], this->NEIGHBOR_RADIUS, nn_indices, nn_dists) > 0)
00273       {
00274         if (this->curvature_estimator == NORMALS)
00275           this->estimateCurvatureAxisNormals(cloud_normals, nn_indices, curvature_axes[i], normals[i]);
00276         else if (this->curvature_estimator == PCA)
00277           this->estimateCurvatureAxisPCA(cloud, r, nn_indices, curvature_axes[i], normals[i]);
00278 
00279         neighborhoods[i] = nn_indices;
00280         neighborhood_centroids[i] = r;
00281       }
00282     }
00283   }
00284   else
00285   {
00286     std::vector<float> nn_dists;
00287     pcl::KdTreeFLANN<pcl::PointXYZRGBA> tree;
00288     tree.setInputCloud(cloud);
00289     std::srand(std::time(0)); // use current time as seed for random generator
00290 
00291     for (int i = 0; i < this->num_samples; i++)
00292     {
00293       // sample random point from the point cloud
00294       int r = std::rand() % cloud->points.size();
00295 
00296       while (!pcl::isFinite((*cloud)[r]) || !this->isPointInWorkspace((*cloud)[r].x, (*cloud)[r].y, (*cloud)[r].z))
00297         r = std::rand() % cloud->points.size();
00298 
00299       // estimate cylinder curvature axis and normal
00300       if (tree.radiusSearch((*cloud)[r], this->NEIGHBOR_RADIUS, nn_indices, nn_dists) > 0)
00301       {
00302         if (this->curvature_estimator == NORMALS)
00303           this->estimateCurvatureAxisNormals(cloud_normals, nn_indices, curvature_axes[i], normals[i]);
00304         else if (this->curvature_estimator == PCA)
00305           this->estimateCurvatureAxisPCA(cloud, r, nn_indices, curvature_axes[i], normals[i]);
00306 
00307         neighborhoods[i] = nn_indices;
00308         neighborhood_centroids[i] = r;
00309       }
00310     }
00311   }
00312 
00313   printf(" elapsed time: %.3f sec\n", omp_get_wtime() - begin_time_axis);
00314 
00315   // define lower and upper bounds on radius of cylinder
00316   double min_radius_cylinder = this->target_radius - this->radius_error;
00317   double max_radius_cylinder = this->target_radius + this->radius_error;
00318 
00319   if (this->use_clearance_filter)
00320     printf("Filtering on curvature, fitting cylinders, and filtering on low clearance ...\n");
00321   else
00322     printf("Filtering on curvature and fitting cylinders ...\n");
00323 
00324   double begin_time = omp_get_wtime();
00325   int cylinders_left_clearance = 0;
00326   Eigen::Vector3d normal;
00327   Eigen::Vector3d curvature_axis;
00328   Eigen::Vector3d curvature_centroid;
00329   std::vector<CylindricalShell> shells;
00330 
00331   double maxHandAperture = this->target_radius + this->radius_error;
00332   double outer_sample_radius = 1.5 * (maxHandAperture + this->handle_gap); // outer sample radius
00333 
00334   // for organized point clouds
00335   pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>::Ptr organized_neighbor(
00336       new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>());
00337   // for unorganized point clouds
00338   pcl::KdTreeFLANN<pcl::PointXYZRGBA> tree;
00339   if (cloud->isOrganized())
00340     organized_neighbor->setInputCloud(cloud);
00341   else
00342     tree.setInputCloud(cloud);
00343 
00344   for (int i = 0; i < this->num_samples; i++)
00345   {
00346     // fit a cylinder to the neighborhood
00347     CylindricalShell shell;
00348     shell.fitCylinder(cloud, neighborhoods[i], normals[i], curvature_axes[i]);
00349 
00350     // set height of shell to 2 * <target_radius>
00351     shell.setExtent(2.0 * this->target_radius);
00352 
00353     // set index of centroid of neighborhood associated with the cylindrical shell
00354     shell.setNeighborhoodCentroidIndex(neighborhood_centroids[i]);
00355 
00356     // check cylinder radius against target radius
00357     if (shell.getRadius() > min_radius_cylinder && shell.getRadius() < max_radius_cylinder)
00358     {
00359       // filter on low clearance
00360       if (this->use_clearance_filter)
00361       {
00362         pcl::PointXYZRGBA searchPoint;
00363         std::vector<int> nn_indices;
00364         std::vector<float> nn_dists;
00365         Eigen::Vector3d centroid = shell.getCentroid();
00366         searchPoint.x = centroid(0);
00367         searchPoint.y = centroid(1);
00368         searchPoint.z = centroid(2);
00369         int num_in_radius = 0;
00370         if (cloud->isOrganized())
00371           num_in_radius = organized_neighbor->radiusSearch(searchPoint, outer_sample_radius, nn_indices, nn_dists);
00372         else
00373           num_in_radius = tree.radiusSearch(searchPoint, outer_sample_radius, nn_indices, nn_dists);
00374 
00375         if ((num_in_radius > 0) && (shell.hasClearance(cloud, nn_indices, maxHandAperture, this->handle_gap)))
00376           shells.push_back(shell);
00377       }
00378       else
00379         shells.push_back(shell);
00380     }
00381   }
00382 
00383   printf(" elapsed time: %.3f sec\n", omp_get_wtime() - begin_time);
00384   if (this->use_clearance_filter)
00385     printf(" cylinders left after clearance filtering: %i\n", (int)shells.size());
00386 
00387   return shells;
00388 }
00389 
00390 std::vector<CylindricalShell> Affordances::searchAffordancesTaubin(const PointCloud::Ptr &cloud,
00391                                                                    tf::StampedTransform *transform)
00392 {
00393   printf("Estimating curvature ...\n");
00394   double beginTime = omp_get_wtime();
00395 
00396   // set-up estimator
00397   pcl::CurvatureEstimationTaubin<pcl::PointXYZRGBA, pcl::PointCurvatureTaubin> estimator;
00398 
00399   // set input source
00400   estimator.setInputCloud(cloud);
00401 
00402   // set radius search
00403   estimator.setRadiusSearch(this->NEIGHBOR_RADIUS);
00404 
00405   // set the number of samples
00406   estimator.setNumSamples(this->num_samples);
00407 
00408   // provide a set of neighborhood centroids
00409   std::vector<int> indices(this->num_samples);
00410   std::srand(std::time(0)); // use current time as seed for random generator
00411   int k;
00412   for (int i = 0; i < this->num_samples; i++)
00413   {
00414     int r = std::rand() % cloud->points.size();
00415     k = 0;
00416     while (!pcl::isFinite((*cloud)[r])
00417         || !this->isPointInWorkspace(cloud->points[r].x, cloud->points[r].y, cloud->points[r].z, transform))
00418     {
00419       r = std::rand() % cloud->points.size();
00420       k++;
00421       if (k == cloud->points.size()) // check that the cloud has finite points
00422       {
00423         printf("No finite points in cloud!\n");
00424         std::vector<CylindricalShell> shells;
00425         shells.resize(0);
00426         return shells;
00427       }
00428     }
00429     indices[i] = r;
00430   }
00431   boost::shared_ptr<std::vector<int> > indices_ptr(new std::vector<int>(indices));
00432   estimator.setIndices(indices_ptr);
00433 
00434   // set number of threads
00435   estimator.setNumThreads(this->num_threads);
00436 
00437   // output dataset
00438   pcl::PointCloud<pcl::PointCurvatureTaubin>::Ptr cloud_curvature(new pcl::PointCloud<pcl::PointCurvatureTaubin>);
00439 
00440   // compute median curvature, normal axis, curvature axis, and curvature centroid
00441   estimator.compute(*cloud_curvature);
00442 
00443   printf(" elapsed time: %.3f sec\n", omp_get_wtime() - beginTime);
00444 
00445   // define lower and upper bounds on radius of osculating sphere and cylinder
00446   double min_radius_osculating_sphere = this->target_radius - 2.0 * this->radius_error;
00447   double max_radius_osculating_sphere = this->target_radius + 2.0 * this->radius_error;
00448   double min_radius_cylinder = this->target_radius - this->radius_error;
00449   double max_radius_cylinder = this->target_radius + this->radius_error;
00450 
00451   if (this->use_clearance_filter)
00452     printf("Filtering on curvature, fitting cylinders, and filtering on low clearance ...\n");
00453   else
00454     printf("Filtering on curvature and fitting cylinders ...\n");
00455 
00456   double begin_time = omp_get_wtime();
00457   int cylinders_left_radius = 0;
00458   int cylinders_left_clearance = 0;
00459   Eigen::Vector3d normal;
00460   Eigen::Vector3d curvature_axis;
00461   std::vector<CylindricalShell> shells;
00462 
00463   double maxHandAperture = this->target_radius + this->radius_error;
00464   double outer_sample_radius = 1.5 * (maxHandAperture + this->handle_gap); // outer sample radius
00465 
00466   // for organized point clouds
00467   pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>::Ptr organized_neighbor(
00468       new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>());
00469   // for unorganized point clouds
00470   pcl::KdTreeFLANN<pcl::PointXYZRGBA> tree;
00471   if (cloud->isOrganized())
00472     organized_neighbor->setInputCloud(cloud);
00473   else
00474     tree.setInputCloud(cloud);
00475 
00476   for (int i = 0; i < cloud_curvature->size(); i++)
00477   {
00478     // calculate radius of osculating sphere
00479     double radius = 1.0 / fabs(cloud_curvature->points[i].median_curvature);
00480 
00481     // filter out planar regions and cylinders that are too large
00482     if (radius > min_radius_osculating_sphere && radius < max_radius_osculating_sphere)
00483     {
00484       // fit a cylinder to the neighborhood
00485       normal << cloud_curvature->points[i].normal_x, cloud_curvature->points[i].normal_y, cloud_curvature->points[i].normal_z;
00486       curvature_axis << cloud_curvature->points[i].curvature_axis_x, cloud_curvature->points[i].curvature_axis_y, cloud_curvature->points[i].curvature_axis_z;
00487       CylindricalShell shell;
00488       shell.fitCylinder(cloud, estimator.getNeighborhoods()[i], normal, curvature_axis);
00489 
00490       // set height of shell to 2 * <target_radius>
00491       shell.setExtent(2.0 * this->target_radius);
00492 
00493       // set index of centroid of neighborhood associated with the cylindrical shell
00494       shell.setNeighborhoodCentroidIndex(estimator.getNeighborhoodCentroids()[i]);
00495 
00496       // check cylinder radius against target radius
00497       if (shell.getRadius() > min_radius_cylinder && shell.getRadius() < max_radius_cylinder)
00498       {
00499         cylinders_left_radius++;
00500 
00501         // filter on low clearance
00502         if (this->use_clearance_filter)
00503         {
00504           pcl::PointXYZRGBA searchPoint;
00505           std::vector<int> nn_indices;
00506           std::vector<float> nn_dists;
00507           Eigen::Vector3d centroid = shell.getCentroid();
00508           searchPoint.x = centroid(0);
00509           searchPoint.y = centroid(1);
00510           searchPoint.z = centroid(2);
00511           int num_in_radius = 0;
00512           if (cloud->isOrganized())
00513             num_in_radius = organized_neighbor->radiusSearch(searchPoint, outer_sample_radius, nn_indices, nn_dists);
00514           else
00515             num_in_radius = tree.radiusSearch(searchPoint, outer_sample_radius, nn_indices, nn_dists);
00516 
00517           if ((num_in_radius > 0) && (shell.hasClearance(cloud, nn_indices, maxHandAperture, this->handle_gap)))
00518             shells.push_back(shell);
00519         }
00520         else
00521           shells.push_back(shell);
00522       }
00523     }
00524   }
00525 
00526   printf(" elapsed time: %.3f sec\n", omp_get_wtime() - begin_time);
00527   printf(" cylinders left after radius filtering: %i\n", cylinders_left_radius);
00528   if (this->use_clearance_filter)
00529     printf(" cylinders left after clearance filtering: %i\n", (int)shells.size());
00530 
00531   return shells;
00532 }
00533 
00534 std::vector<std::vector<CylindricalShell> > Affordances::searchHandles(const PointCloud::Ptr &cloud,
00535                                                                        std::vector<CylindricalShell> shells)
00536 {
00537   std::vector < std::vector<CylindricalShell> > handles;
00538 
00539   // find colinear sets of cylinders
00540   if (this->alignment_runs > 0)
00541   {
00542     std::cout << "alignment search for colinear sets of cylinders (handles) ... " << std::endl;
00543     double beginTime = omp_get_wtime();
00544     std::vector<int> inliersMaxSet, outliersMaxSet;
00545 
00546     // linear search
00547     for (int i = 0; i < this->alignment_runs && shells.size() > 0; i++)
00548     {
00549       this->findBestColinearSet(shells, inliersMaxSet, outliersMaxSet);
00550       printf(" number of inliers in run %i: %i", i, (int)inliersMaxSet.size());
00551 
00552       if (inliersMaxSet.size() >= this->alignment_min_inliers)
00553       {
00554         // create handle from inlier indices
00555         std::vector<CylindricalShell> handle;
00556         for (std::size_t j = 0; j < inliersMaxSet.size(); j++)
00557         {
00558           int idx = inliersMaxSet[j];
00559           handle.push_back(shells[idx]);
00560         }
00561 
00562         // check for occlusions
00563         if (this->use_occlusion_filter)
00564         {
00565           int MAX_NUM_OCCLUDED = (int)handle.size() * 0.5; // 5
00566           int num_occluded = 0;
00567           bool is_occluded = false;
00568 
00569           for (std::size_t j = 0; j < handle.size(); j++)
00570           {
00571             if (this->numInFront(cloud, handle[j].getNeighborhoodCentroidIndex(),
00572                                  1.5 * this->target_radius + this->radius_error) > this->MAX_NUM_IN_FRONT)
00573             {
00574               num_occluded++;
00575               if (num_occluded > MAX_NUM_OCCLUDED)
00576               {
00577                 is_occluded = true;
00578                 break;
00579               }
00580             }
00581           }
00582 
00583           printf("  number of occluded affordances: %i; occluded: %s\n", num_occluded, is_occluded ? "true" : "false");
00584 
00585           if (!is_occluded)
00586             handles.push_back(handle);
00587         }
00588         else
00589         {
00590           handles.push_back(handle);
00591         }
00592 
00593         // prune list of cylindrical shells
00594         std::vector<CylindricalShell> remainder(outliersMaxSet.size());
00595         for (std::size_t j = 0; j < outliersMaxSet.size(); j++)
00596           remainder[j] = shells[outliersMaxSet[j]];
00597         shells = remainder;
00598         printf(", remaining cylinders: %i\n", (int)shells.size());
00599       }
00600       // do not check for occlusions
00601       else
00602       {
00603         break;
00604       }
00605     }
00606 
00607     printf(" elapsed time: %.3f\n", omp_get_wtime() - beginTime);
00608   }
00609 
00610   return handles;
00611 }
00612 
00613 std::vector<CylindricalShell> Affordances::searchAffordances(const PointCloud::Ptr &cloud,
00614                                                              const std::vector<int> &indices)
00615 {
00616   Eigen::MatrixXd samples(3, indices.size());
00617   for (std::size_t i = 0; i < indices.size(); i++)
00618     samples.col(i) = cloud->points[indices[i]].getVector3fMap().cast<double>();
00619 
00620   return this->searchAffordancesTaubin(cloud, samples);
00621 }
00622 
00623 std::vector<CylindricalShell> Affordances::searchAffordancesTaubin(const PointCloud::Ptr &cloud,
00624                                                                    const Eigen::MatrixXd &samples, bool is_logging)
00625 {
00626   if (is_logging)
00627     printf("Estimating curvature ...\n");
00628 
00629   double beginTime = omp_get_wtime();
00630 
00631   // set-up estimator
00632   pcl::PointCloud<pcl::PointCurvatureTaubin>::Ptr cloud_curvature(new pcl::PointCloud<pcl::PointCurvatureTaubin>);
00633   pcl::CurvatureEstimationTaubin<pcl::PointXYZRGBA, pcl::PointCurvatureTaubin> estimator;
00634   estimator.setInputCloud(cloud);
00635   estimator.setRadiusSearch(this->NEIGHBOR_RADIUS);
00636   //~ estimator.setRadiusSearch(1.5*target_radius + radius_error);
00637   estimator.setNumThreads(this->num_threads);
00638 
00639   // compute median curvature, normal axis, curvature axis, and curvature centroid
00640   estimator.computeFeature(samples, *cloud_curvature);
00641 
00642   if (is_logging)
00643     printf(" elapsed time: %.3f sec, cylinders left: %i\n", omp_get_wtime() - beginTime,
00644            (int)cloud_curvature->points.size());
00645 
00646   // define lower and upper bounds on radius of osculating sphere and cylinder
00647   double min_radius_osculating_sphere = this->target_radius - 2.0 * this->radius_error;
00648   double max_radius_osculating_sphere = this->target_radius + 2.0 * this->radius_error;
00649   double min_radius_cylinder = this->target_radius - this->radius_error;
00650   double max_radius_cylinder = this->target_radius + this->radius_error;
00651 
00652   if (is_logging && this->use_clearance_filter)
00653     printf("Filtering on curvature, fitting cylinders, and filtering on low clearance ...\n");
00654   else if (is_logging)
00655     printf("Filtering on curvature and fitting cylinders ...\n");
00656 
00657   double begin_time = omp_get_wtime();
00658   int cylinders_left_radius = 0;
00659   int cylinders_left_clearance = 0;
00660   Eigen::Vector3d normal;
00661   Eigen::Vector3d curvature_axis;
00662   std::vector<CylindricalShell> shells;
00663   double tcyltotal = 0.0;
00664   double tcleartotal = 0.0;
00665 
00666   double maxHandAperture = this->target_radius + this->radius_error;
00667   double outer_sample_radius = 1.5 * (maxHandAperture + this->handle_gap); // outer sample radius
00668 
00669   // for organized point clouds
00670   pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>::Ptr organized_neighbor(
00671       new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>());
00672   // for unorganized point clouds
00673   pcl::KdTreeFLANN<pcl::PointXYZRGBA> tree;
00674   if (cloud->isOrganized())
00675     organized_neighbor->setInputCloud(cloud);
00676   else
00677     tree.setInputCloud(cloud);
00678 
00679   //~ #ifdef _OPENMP
00680   //~ #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)
00681   //~ #endif
00682   for (int i = 0; i < cloud_curvature->size(); i++)
00683   {
00684     if (isnan(cloud_curvature->points[i].normal[0]))
00685       continue;
00686 
00687     // calculate radius of osculating sphere
00688     double radius = 1.0 / fabs(cloud_curvature->points[i].median_curvature);
00689     //~ printf("%i: radius of osculating sphere: %.4f\n", i, radius);
00690 
00691     // filter out planar regions and cylinders that are too large
00692     if (radius > min_radius_osculating_sphere && radius < max_radius_osculating_sphere)
00693     {
00694       // fit a cylinder to the neighborhood
00695       double tcyl0 = omp_get_wtime();
00696       normal << cloud_curvature->points[i].normal_x, cloud_curvature->points[i].normal_y, cloud_curvature->points[i].normal_z;
00697       curvature_axis << cloud_curvature->points[i].curvature_axis_x, cloud_curvature->points[i].curvature_axis_y, cloud_curvature->points[i].curvature_axis_z;
00698       CylindricalShell shell;
00699       shell.fitCylinder(cloud, estimator.getNeighborhoods()[i], normal, curvature_axis);
00700       tcyltotal += omp_get_wtime() - tcyl0;
00701 
00702       //~ printf(" radius of fitted cylinder: %.4f\n", shell.getRadius());
00703 
00704       // set height of shell to 2 * <target_radius>
00705       shell.setExtent(2.0 * this->target_radius);
00706 
00707       // set index of centroid of neighborhood associated with the cylindrical shell
00708       shell.setNeighborhoodCentroidIndex(estimator.getNeighborhoodCentroids()[i]);
00709 
00710       // check cylinder radius against target radius
00711       if (shell.getRadius() > min_radius_cylinder && shell.getRadius() < max_radius_cylinder)
00712       {
00713         cylinders_left_radius++;
00714 
00715         // filter on low clearance
00716         if (this->use_clearance_filter)
00717         {
00718           double tclear0 = omp_get_wtime();
00719           pcl::PointXYZRGBA searchPoint;
00720           std::vector<int> nn_indices;
00721           std::vector<float> nn_dists;
00722           Eigen::Vector3d centroid = shell.getCentroid();
00723           searchPoint.x = centroid(0);
00724           searchPoint.y = centroid(1);
00725           searchPoint.z = centroid(2);
00726           int num_in_radius = 0;
00727           if (cloud->isOrganized())
00728             num_in_radius = organized_neighbor->radiusSearch(searchPoint, outer_sample_radius, nn_indices, nn_dists);
00729           else
00730             num_in_radius = tree.radiusSearch(searchPoint, outer_sample_radius, nn_indices, nn_dists);
00731 
00732           if ((num_in_radius > 0) && (shell.hasClearance(cloud, nn_indices, maxHandAperture, this->handle_gap)))
00733             shells.push_back(shell);
00734 
00735           tcleartotal += omp_get_wtime() - tclear0;
00736         }
00737         else
00738           shells.push_back(shell);
00739       }
00740     }
00741   }
00742 
00743   if (is_logging)
00744   {
00745     printf(" elapsed time: %.3f sec\n", omp_get_wtime() - begin_time);
00746     printf(" cylinders left after radius filtering: %i\n", cylinders_left_radius);
00747     if (this->use_clearance_filter)
00748       printf(" cylinders left after clearance filtering: %i\n", (int)shells.size());
00749     printf("  cylinder/circle fitting: %.3f sec\n", tcyltotal);
00750     printf("  shell search: %.3f sec\n", tcleartotal);
00751   }
00752 
00753   return shells;
00754 }
00755 
00756 std::vector<int> Affordances::createRandomIndices(const PointCloud::Ptr &cloud, int size)
00757 {
00758   std::vector<int> indices(size);
00759 
00760   for (int i = 0; i < size; i++)
00761   {
00762     int r = std::rand() % cloud->points.size();
00763     while (!pcl::isFinite((*cloud)[r])
00764         || !this->isPointInWorkspace(cloud->points[r].x, cloud->points[r].y, cloud->points[r].z))
00765       r = std::rand() % cloud->points.size();
00766     indices[i] = r;
00767   }
00768 
00769   return indices;
00770 }
00771 
00772 void Affordances::findBestColinearSet(const std::vector<CylindricalShell> &list, std::vector<int> &inliersMaxSet,
00773                                       std::vector<int> &outliersMaxSet)
00774 {
00775   int maxInliers = 0;
00776   double orientRadius2 = this->alignment_orient_radius * this->alignment_orient_radius;
00777   double distRadius2 = this->alignment_dist_radius * this->alignment_dist_radius;
00778 
00779   for (std::size_t i = 0; i < list.size(); i++)
00780   {
00781     Eigen::Vector3d axis = list[i].getCurvatureAxis();
00782     Eigen::Vector3d centroid = list[i].getCentroid();
00783     double radius = list[i].getRadius();
00784     std::vector<int> inliers, outliers;
00785 
00786     for (std::size_t j = 0; j < list.size(); j++)
00787     {
00788       Eigen::Vector3d distToOrientVec = (Eigen::MatrixXd::Identity(3, 3) - axis * axis.transpose())
00789           * list[j].getCurvatureAxis();
00790       double distToOrient = distToOrientVec.cwiseProduct(distToOrientVec).sum();
00791       Eigen::Vector3d distToAxisVec = (Eigen::MatrixXd::Identity(3, 3) - axis * axis.transpose())
00792           * (list[j].getCentroid() - centroid);
00793       double distToAxis = distToAxisVec.cwiseProduct(distToAxisVec).sum();
00794       double distToRadius = fabs(list[j].getRadius() - radius);
00795 
00796       if (distToOrient < orientRadius2 && distToAxis < distRadius2 && distToRadius < this->alignment_radius_radius)
00797         inliers.push_back(j);
00798       else
00799         outliers.push_back(j);
00800     }
00801 
00802     if (inliers.size() > maxInliers)
00803     {
00804       maxInliers = inliers.size();
00805       inliersMaxSet = inliers;
00806       outliersMaxSet = outliers;
00807     }
00808   }
00809 }


handle_detector
Author(s):
autogenerated on Thu Jun 6 2019 17:36:23