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
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
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
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
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
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));
00262
00263 for (int i = 0; i < this->num_samples; i++)
00264 {
00265
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
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));
00290
00291 for (int i = 0; i < this->num_samples; i++)
00292 {
00293
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
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
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);
00333
00334
00335 pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>::Ptr organized_neighbor(
00336 new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>());
00337
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
00347 CylindricalShell shell;
00348 shell.fitCylinder(cloud, neighborhoods[i], normals[i], curvature_axes[i]);
00349
00350
00351 shell.setExtent(2.0 * this->target_radius);
00352
00353
00354 shell.setNeighborhoodCentroidIndex(neighborhood_centroids[i]);
00355
00356
00357 if (shell.getRadius() > min_radius_cylinder && shell.getRadius() < max_radius_cylinder)
00358 {
00359
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
00397 pcl::CurvatureEstimationTaubin<pcl::PointXYZRGBA, pcl::PointCurvatureTaubin> estimator;
00398
00399
00400 estimator.setInputCloud(cloud);
00401
00402
00403 estimator.setRadiusSearch(this->NEIGHBOR_RADIUS);
00404
00405
00406 estimator.setNumSamples(this->num_samples);
00407
00408
00409 std::vector<int> indices(this->num_samples);
00410 std::srand(std::time(0));
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())
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
00435 estimator.setNumThreads(this->num_threads);
00436
00437
00438 pcl::PointCloud<pcl::PointCurvatureTaubin>::Ptr cloud_curvature(new pcl::PointCloud<pcl::PointCurvatureTaubin>);
00439
00440
00441 estimator.compute(*cloud_curvature);
00442
00443 printf(" elapsed time: %.3f sec\n", omp_get_wtime() - beginTime);
00444
00445
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);
00465
00466
00467 pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>::Ptr organized_neighbor(
00468 new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>());
00469
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
00479 double radius = 1.0 / fabs(cloud_curvature->points[i].median_curvature);
00480
00481
00482 if (radius > min_radius_osculating_sphere && radius < max_radius_osculating_sphere)
00483 {
00484
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
00491 shell.setExtent(2.0 * this->target_radius);
00492
00493
00494 shell.setNeighborhoodCentroidIndex(estimator.getNeighborhoodCentroids()[i]);
00495
00496
00497 if (shell.getRadius() > min_radius_cylinder && shell.getRadius() < max_radius_cylinder)
00498 {
00499 cylinders_left_radius++;
00500
00501
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
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
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
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
00563 if (this->use_occlusion_filter)
00564 {
00565 int MAX_NUM_OCCLUDED = (int)handle.size() * 0.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
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
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
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
00637 estimator.setNumThreads(this->num_threads);
00638
00639
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
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);
00668
00669
00670 pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>::Ptr organized_neighbor(
00671 new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>());
00672
00673 pcl::KdTreeFLANN<pcl::PointXYZRGBA> tree;
00674 if (cloud->isOrganized())
00675 organized_neighbor->setInputCloud(cloud);
00676 else
00677 tree.setInputCloud(cloud);
00678
00679
00680
00681
00682 for (int i = 0; i < cloud_curvature->size(); i++)
00683 {
00684 if (isnan(cloud_curvature->points[i].normal[0]))
00685 continue;
00686
00687
00688 double radius = 1.0 / fabs(cloud_curvature->points[i].median_curvature);
00689
00690
00691
00692 if (radius > min_radius_osculating_sphere && radius < max_radius_osculating_sphere)
00693 {
00694
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
00703
00704
00705 shell.setExtent(2.0 * this->target_radius);
00706
00707
00708 shell.setNeighborhoodCentroidIndex(estimator.getNeighborhoodCentroids()[i]);
00709
00710
00711 if (shell.getRadius() > min_radius_cylinder && shell.getRadius() < max_radius_cylinder)
00712 {
00713 cylinders_left_radius++;
00714
00715
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 }