CloudPtr get () { //lock while we swap our cloud and reset it. boost::mutex::scoped_lock lock (mtx_); CloudPtr temp_cloud (new Cloud); CloudPtr temp_cloud2 (new Cloud); CloudPtr temp_cloud3 (new Cloud); CloudPtr temp_cloud4 (new Cloud); CloudPtr temp_cloud5 (new Cloud); CloudConstPtr empty_cloud; cout << "===============================\n" "======Start of frame===========\n" "===============================\n"; //cerr << "cloud size orig: " << cloud_->size() << endl; voxel_grid.setInputCloud (cloud_); voxel_grid.filter (*temp_cloud); // filter cloud for z depth //cerr << "cloud size postzfilter: " << temp_cloud->size() << endl; pcl::ModelCoefficients::Ptr planecoefficients (new pcl::ModelCoefficients ()); pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices ()); std::vector<pcl::ModelCoefficients> linecoefficients1; pcl::ModelCoefficients model; model.values.resize (6); pcl::PointIndices::Ptr line_inliers (new pcl::PointIndices ()); std::vector<Eigen::Vector3f> corners; if(temp_cloud->size() > MIN_CLOUD_POINTS) { plane_seg.setInputCloud (temp_cloud); plane_seg.segment (*plane_inliers, *planecoefficients); // find plane } //cerr << "plane inliers size: " << plane_inliers->indices.size() << endl; cout << "planecoeffs: " << planecoefficients->values[0] << " " << planecoefficients->values[1] << " " << planecoefficients->values[2] << " " << planecoefficients->values[3] << " " << endl; Eigen::Vector3f pn = Eigen::Vector3f( planecoefficients->values[0], planecoefficients->values[1], planecoefficients->values[2]); float planedeg = pcl::rad2deg(acos(pn.dot(Eigen::Vector3f::UnitZ()))); cout << "angle of camera to floor normal: " << planedeg << " degrees" << endl; cout << "distance of camera to floor: " << planecoefficients->values[3] << " meters" << endl; plane_extract.setNegative (true); plane_extract.setInputCloud (temp_cloud); plane_extract.setIndices (plane_inliers); plane_extract.filter (*temp_cloud2); // remove plane plane_extract.setNegative (false); plane_extract.filter (*temp_cloud5); // only plane for(size_t i = 0; i < temp_cloud5->size (); ++i) { temp_cloud5->points[i].r = 0; temp_cloud5->points[i].g = 255; temp_cloud5->points[i].b = 0; // tint found plane green for ground } for(size_t j = 0 ; j < MAX_LINES && temp_cloud2->size() > MIN_CLOUD_POINTS; j++) // look for x lines until cloud gets too small { // cerr << "cloud size: " << temp_cloud2->size() << endl; line_seg.setInputCloud (temp_cloud2); line_seg.segment (*line_inliers, model); // find line // cerr << "line inliears size: " << line_inliers->indices.size() << endl; if(line_inliers->indices.size() < MIN_CLOUD_POINTS) break; linecoefficients1.push_back (model); // store line coeffs line_extract.setNegative (true); line_extract.setInputCloud (temp_cloud2); line_extract.setIndices (line_inliers); line_extract.filter (*temp_cloud3); // remove plane line_extract.setNegative (false); line_extract.filter (*temp_cloud4); // only plane for(size_t i = 0; i < temp_cloud4->size (); ++i) { temp_cloud4->points[i].g = 0; if(j%2) { temp_cloud4->points[i].r = 255-j*int(255/MAX_LINES); temp_cloud4->points[i].b = 0+j*int(255/MAX_LINES); } else { temp_cloud4->points[i].b = 255-j*int(255/MAX_LINES); temp_cloud4->points[i].r = 0+j*int(255/MAX_LINES); } } *temp_cloud5 += *temp_cloud4; // add line to ground temp_cloud2.swap ( temp_cloud3); // remove line } cout << "found " << linecoefficients1.size() << " lines." << endl; for(size_t i = 0; i < linecoefficients1.size(); i++) { //cerr << "linecoeffs: " << i << " " // << linecoefficients1[i].values[0] << " " // << linecoefficients1[i].values[1] << " " // << linecoefficients1[i].values[2] << " " // << linecoefficients1[i].values[3] << " " // << linecoefficients1[i].values[4] << " " // << linecoefficients1[i].values[5] << " " // << endl; Eigen::Vector3f lv = Eigen::Vector3f( linecoefficients1[i].values[3], linecoefficients1[i].values[4], linecoefficients1[i].values[5]); Eigen::Vector3f lp = Eigen::Vector3f( linecoefficients1[i].values[0], linecoefficients1[i].values[1], linecoefficients1[i].values[2]); float r = pn.dot(lv); float deg = pcl::rad2deg(acos(r)); cout << "angle of line to floor normal: " << deg << " degrees" << endl; if(abs(deg-30) < 5 || abs(deg-150) < 5) { cout << "found corner line" << endl; float t = -(lp.dot(pn) + planecoefficients->values[3])/ r; Eigen::Vector3f intersect = lp + lv*t; cout << "corner intersects floor at: " << endl << intersect << endl; cout << "straight line distance from camera to corner: " << intersect.norm() << " meters" << endl; corners.push_back(intersect); Eigen::Vector3f floor_distance = intersect + pn; // should be - ??? cout << "distance along floor to corner: " << floor_distance.norm() << " meters" << endl; } else if(abs(deg-90) < 5) { cout << "found horizontal line" << endl; } } switch(corners.size()) { case 2: cout << "distance between corners " << (corners[0] - corners[1]).norm() << endl; cout << "angle of pyramid to camera " << pcl::rad2deg(acos(((corners[0] - corners[1]).normalized()).dot(Eigen::Vector3f::UnitX()))) << endl; break; case 3: cout << "distance between corners " << (corners[0] - corners[1]).norm() << endl; cout << "distance between corners " << (corners[0] - corners[2]).norm() << endl; cout << "distance between corners " << (corners[1] - corners[2]).norm() << endl; cout << "angle of corner on floor (should be 90) " << pcl::rad2deg(acos((corners[0] - corners[1]).dot(corners[0] - corners [2]))) << endl; cout << "angle of pyramid to camera " << pcl::rad2deg(acos(((corners[0] - corners[1]).normalized()).dot(Eigen::Vector3f::UnitX()))) << endl; break; } if (saveCloud) { std::stringstream stream, stream1; std::string filename; stream << "inputCloud" << filesSaved << ".pcd"; filename = stream.str(); if (pcl::io::savePCDFile(filename, *cloud_, true) == 0) { filesSaved++; cout << "Saved " << filename << "." << endl; } else PCL_ERROR("Problem saving %s.\n", filename.c_str()); stream1 << "inputCloud" << filesSaved << ".pcd"; filename = stream1.str(); if (pcl::io::savePCDFile(filename, *temp_cloud5, true) == 0) { filesSaved++; cout << "Saved " << filename << "." << endl; } else PCL_ERROR("Problem saving %s.\n", filename.c_str()); saveCloud = false; } empty_cloud.swap(cloud_); // set cloud_ to null if(toggleView == 1) return (temp_cloud); // return orig cloud else return (temp_cloud5); // return colored cloud }
bool SnapIt::processModelCylinder(jsk_pcl_ros::CallSnapIt::Request& req, jsk_pcl_ros::CallSnapIt::Response& res) { pcl::PointCloud<pcl::PointXYZ>::Ptr candidate_points (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); Eigen::Vector3f n, C_orig; if (!extractPointsInsideCylinder(req.request.center, req.request.direction, req.request.radius, req.request.height, inliers, n, C_orig, 1.3)) { return false; } if (inliers->indices.size() < 3) { NODELET_ERROR("not enough points inside of the target_plane"); return false; } geometry_msgs::PointStamped centroid; centroid.point.x = C_orig[0]; centroid.point.y = C_orig[1]; centroid.point.z = C_orig[2]; centroid.header = req.request.header; pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(input_); extract.setIndices(inliers); extract.filter(*candidate_points); publishPointCloud(debug_candidate_points_pub_, candidate_points); // first, to remove plane we estimate the plane pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices); extractPlanePoints(candidate_points, plane_inliers, plane_coefficients, n, req.request.eps_angle); if (plane_inliers->indices.size() == 0) { NODELET_ERROR ("plane estimation failed"); return false; } // remove the points blonging to the plane pcl::PointCloud<pcl::PointXYZ>::Ptr points_inside_pole_wo_plane (new pcl::PointCloud<pcl::PointXYZ>); extract.setInputCloud (candidate_points); extract.setIndices (plane_inliers); extract.setNegative (true); extract.filter (*points_inside_pole_wo_plane); publishPointCloud(debug_candidate_points_pub2_, points_inside_pole_wo_plane); // normal estimation pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne; pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ()); ne.setSearchMethod (tree); ne.setInputCloud (points_inside_pole_wo_plane); ne.setKSearch (50); ne.compute (*cloud_normals); // segmentation pcl::ModelCoefficients::Ptr cylinder_coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr cylinder_inliers (new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_CYLINDER); seg.setMethodType (pcl::SAC_RANSAC); seg.setRadiusLimits (0.01, req.request.radius * 1.2); seg.setDistanceThreshold (0.05); seg.setInputCloud(points_inside_pole_wo_plane); seg.setInputNormals (cloud_normals); seg.setMaxIterations (10000); seg.setNormalDistanceWeight (0.1); seg.setAxis(n); if (req.request.eps_angle != 0.0) { seg.setEpsAngle(req.request.eps_angle); } else { seg.setEpsAngle(0.35); } seg.segment (*cylinder_inliers, *cylinder_coefficients); if (cylinder_inliers->indices.size () == 0) { NODELET_ERROR ("Could not estimate a cylinder model for the given dataset."); return false; } debug_centroid_pub_.publish(centroid); pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_points (new pcl::PointCloud<pcl::PointXYZ>); extract.setInputCloud (points_inside_pole_wo_plane); extract.setIndices (cylinder_inliers); extract.setNegative (false); extract.filter (*cylinder_points); publishPointCloud(debug_candidate_points_pub3_, cylinder_points); Eigen::Vector3f n_prime; Eigen::Vector3f C_new; for (size_t i = 0; i < 3; i++) { C_new[i] = cylinder_coefficients->values[i]; n_prime[i] = cylinder_coefficients->values[i + 3]; } double radius = fabs(cylinder_coefficients->values[6]); // inorder to compute centroid, we project all the points to the center line. // and after that, get the minimum and maximum points in the coordinate system of the center line double min_alpha = DBL_MAX; double max_alpha = -DBL_MAX; for (size_t i = 0; i < cylinder_points->points.size(); i++ ) { pcl::PointXYZ q = cylinder_points->points[i]; double alpha = (q.getVector3fMap() - C_new).dot(n_prime); if (alpha < min_alpha) { min_alpha = alpha; } if (alpha > max_alpha) { max_alpha = alpha; } } // the center of cylinder Eigen::Vector3f C_new_prime = C_new + (max_alpha + min_alpha) / 2.0 * n_prime; Eigen::Vector3f n_cross = n.cross(n_prime); if (n.dot(n_prime)) { n_cross = - n_cross; } double theta = asin(n_cross.norm()); Eigen::Quaternionf trans (Eigen::AngleAxisf(theta, n_cross.normalized())); Eigen::Vector3f C = trans * C_orig; Eigen::Affine3f A = Eigen::Translation3f(C) * Eigen::Translation3f(C_new_prime - C) * trans * Eigen::Translation3f(C_orig).inverse(); tf::poseEigenToMsg((Eigen::Affine3d)A, res.transformation); geometry_msgs::PointStamped centroid_transformed; centroid_transformed.point.x = C_new_prime[0]; centroid_transformed.point.y = C_new_prime[1]; centroid_transformed.point.z = C_new_prime[2]; centroid_transformed.header = req.request.header; debug_centroid_after_trans_pub_.publish(centroid_transformed); // publish marker visualization_msgs::Marker marker; marker.type = visualization_msgs::Marker::CYLINDER; marker.scale.x = radius; marker.scale.y = radius; marker.scale.z = (max_alpha - min_alpha); marker.pose.position.x = C_new_prime[0]; marker.pose.position.y = C_new_prime[1]; marker.pose.position.z = C_new_prime[2]; // n_prime -> z // n_cross.normalized() -> x Eigen::Vector3f z_axis = n_prime.normalized(); Eigen::Vector3f y_axis = n_cross.normalized(); Eigen::Vector3f x_axis = (y_axis.cross(z_axis)).normalized(); Eigen::Matrix3f M; for (size_t i = 0; i < 3; i++) { M(i, 0) = x_axis[i]; M(i, 1) = y_axis[i]; M(i, 2) = z_axis[i]; } Eigen::Quaternionf q (M); marker.pose.orientation.x = q.x(); marker.pose.orientation.y = q.y(); marker.pose.orientation.z = q.z(); marker.pose.orientation.w = q.w(); marker.color.g = 1.0; marker.color.a = 1.0; marker.header = input_header_; marker_pub_.publish(marker); return true; }
void RegionGrowingMultiplePlaneSegmentation::segment( const sensor_msgs::PointCloud2::ConstPtr& msg, const sensor_msgs::PointCloud2::ConstPtr& normal_msg) { boost::mutex::scoped_lock lock(mutex_); if (!done_initialization_) { return; } vital_checker_->poke(); { jsk_recognition_utils::ScopedWallDurationReporter r = timer_.reporter(pub_latest_time_, pub_average_time_); pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>); pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>); pcl::fromROSMsg(*msg, *cloud); pcl::fromROSMsg(*normal_msg, *normal); // concatenate fields pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr all_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>); pcl::concatenateFields(*cloud, *normal, *all_cloud); pcl::PointIndices::Ptr indices (new pcl::PointIndices); for (size_t i = 0; i < all_cloud->points.size(); i++) { if (!isnan(all_cloud->points[i].x) && !isnan(all_cloud->points[i].y) && !isnan(all_cloud->points[i].z) && !isnan(all_cloud->points[i].normal_x) && !isnan(all_cloud->points[i].normal_y) && !isnan(all_cloud->points[i].normal_z) && !isnan(all_cloud->points[i].curvature)) { if (all_cloud->points[i].curvature < max_curvature_) { indices->indices.push_back(i); } } } pcl::ConditionalEuclideanClustering<pcl::PointXYZRGBNormal> cec (true); // vector of pcl::PointIndices pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters); cec.setInputCloud (all_cloud); cec.setIndices(indices); cec.setClusterTolerance(cluster_tolerance_); cec.setMinClusterSize(min_size_); //cec.setMaxClusterSize(max_size_); { boost::mutex::scoped_lock lock2(global_custom_condigion_function_mutex); setCondifionFunctionParameter(angular_threshold_, distance_threshold_); cec.setConditionFunction( &RegionGrowingMultiplePlaneSegmentation::regionGrowingFunction); //ros::Time before = ros::Time::now(); cec.segment (*clusters); // ros::Time end = ros::Time::now(); // ROS_INFO("segment took %f sec", (before - end).toSec()); } // Publish raw cluster information // pcl::IndicesCluster is typdefed as std::vector<pcl::PointIndices> jsk_recognition_msgs::ClusterPointIndices ros_clustering_result; ros_clustering_result.cluster_indices = pcl_conversions::convertToROSPointIndices(*clusters, msg->header); ros_clustering_result.header = msg->header; pub_clustering_result_.publish(ros_clustering_result); // estimate planes std::vector<pcl::PointIndices::Ptr> all_inliers; std::vector<pcl::ModelCoefficients::Ptr> all_coefficients; jsk_recognition_msgs::PolygonArray ros_polygon; ros_polygon.header = msg->header; for (size_t i = 0; i < clusters->size(); i++) { pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices); pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients); pcl::PointIndices::Ptr cluster = boost::make_shared<pcl::PointIndices>((*clusters)[i]); ransacEstimation(cloud, cluster, *plane_inliers, *plane_coefficients); if (plane_inliers->indices.size() > 0) { jsk_recognition_utils::ConvexPolygon::Ptr convex = jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointXYZRGB>( cloud, plane_inliers, plane_coefficients); if (convex) { if (min_area_ > convex->area() || convex->area() > max_area_) { continue; } { // check direction consistency of coefficients and convex Eigen::Vector3f coefficient_normal(plane_coefficients->values[0], plane_coefficients->values[1], plane_coefficients->values[2]); if (convex->getNormalFromVertices().dot(coefficient_normal) < 0) { convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex()); } } // Normal should direct to origin { Eigen::Vector3f p = convex->getPointOnPlane(); Eigen::Vector3f n = convex->getNormal(); if (p.dot(n) > 0) { convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex()); Eigen::Vector3f new_normal = convex->getNormal(); plane_coefficients->values[0] = new_normal[0]; plane_coefficients->values[1] = new_normal[1]; plane_coefficients->values[2] = new_normal[2]; plane_coefficients->values[3] = convex->getD(); } } geometry_msgs::PolygonStamped polygon; polygon.polygon = convex->toROSMsg(); polygon.header = msg->header; ros_polygon.polygons.push_back(polygon); all_inliers.push_back(plane_inliers); all_coefficients.push_back(plane_coefficients); } } } jsk_recognition_msgs::ClusterPointIndices ros_indices; ros_indices.cluster_indices = pcl_conversions::convertToROSPointIndices(all_inliers, msg->header); ros_indices.header = msg->header; pub_inliers_.publish(ros_indices); jsk_recognition_msgs::ModelCoefficientsArray ros_coefficients; ros_coefficients.header = msg->header; ros_coefficients.coefficients = pcl_conversions::convertToROSModelCoefficients( all_coefficients, msg->header); pub_coefficients_.publish(ros_coefficients); pub_polygons_.publish(ros_polygon); } }
bool SnapIt::processModelPlane(jsk_pcl_ros::CallSnapIt::Request& req, jsk_pcl_ros::CallSnapIt::Response& res) { // first build plane model geometry_msgs::PolygonStamped target_plane = req.request.target_plane; // check the size of the points if (target_plane.polygon.points.size() < 3) { NODELET_ERROR("not enough points included in target_plane"); return false; } pcl::PointCloud<pcl::PointXYZ>::Ptr points_inside_pole (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); EigenVector3fVector points; Eigen::Vector3f n, p; if (!extractPointsInsidePlanePole(target_plane, inliers, points, n, p)) { return false; } if (inliers->indices.size() < 3) { NODELET_ERROR("not enough points inside of the target_plane"); return false; } pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(input_); extract.setIndices(inliers); extract.filter(*points_inside_pole); publishPointCloud(debug_candidate_points_pub_, points_inside_pole); // estimate plane pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices); extractPlanePoints(points_inside_pole, plane_inliers, plane_coefficients, n, req.request.eps_angle); if (plane_inliers->indices.size () == 0) { NODELET_ERROR ("Could not estimate a planar model for the given dataset."); return false; } // extract plane points pcl::PointCloud<pcl::PointXYZ>::Ptr plane_points (new pcl::PointCloud<pcl::PointXYZ>); pcl::ProjectInliers<pcl::PointXYZ> proj; proj.setModelType (pcl::SACMODEL_PLANE); proj.setIndices (plane_inliers); proj.setInputCloud (points_inside_pole); proj.setModelCoefficients (plane_coefficients); proj.filter (*plane_points); publishPointCloud(debug_candidate_points_pub3_, plane_points); // next, compute convexhull and centroid pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>); pcl::ConcaveHull<pcl::PointXYZ> chull; chull.setInputCloud (plane_points); chull.setDimension(2); chull.setAlpha (0.1); chull.reconstruct (*cloud_hull); if (cloud_hull->points.size() < 3) { NODELET_ERROR("failed to estimate convex hull"); return false; } publishConvexHullMarker(cloud_hull); Eigen::Vector4f C_new_4f; pcl::compute3DCentroid(*cloud_hull, C_new_4f); Eigen::Vector3f C_new; for (size_t i = 0; i < 3; i++) { C_new[i] = C_new_4f[i]; } Eigen::Vector3f n_prime; n_prime[0] = plane_coefficients->values[0]; n_prime[1] = plane_coefficients->values[1]; n_prime[2] = plane_coefficients->values[2]; plane_coefficients->values[3] = plane_coefficients->values[3] / n_prime.norm(); n_prime.normalize(); if (n_prime.dot(n) < 0) { n_prime = - n_prime; plane_coefficients->values[3] = - plane_coefficients->values[3]; } Eigen::Vector3f n_cross = n.cross(n_prime); double theta = asin(n_cross.norm()); Eigen::Quaternionf trans (Eigen::AngleAxisf(theta, n_cross.normalized())); // compute C Eigen::Vector3f C_orig(0, 0, 0); for (size_t i = 0; i < points.size(); i++) { C_orig = C_orig + points[i]; } C_orig = C_orig / points.size(); // compute C Eigen::Vector3f C = trans * C_orig; Eigen::Affine3f A = Eigen::Translation3f(C) * Eigen::Translation3f(C_new - C) * trans * Eigen::Translation3f(C_orig).inverse(); tf::poseEigenToMsg((Eigen::Affine3d)A, res.transformation); geometry_msgs::PointStamped centroid; centroid.point.x = C_orig[0]; centroid.point.y = C_orig[1]; centroid.point.z = C_orig[2]; centroid.header = target_plane.header; debug_centroid_pub_.publish(centroid); geometry_msgs::PointStamped centroid_transformed; centroid_transformed.point.x = C_new[0]; centroid_transformed.point.y = C_new[1]; centroid_transformed.point.z = C_new[2]; centroid_transformed.header = target_plane.header; debug_centroid_after_trans_pub_.publish(centroid_transformed); return true; }
/** * extract handles from a pointcloud * @param[pointCloudIn] input point cloud * @param[pointCloudNormals] normals for the input cloud * @param[handle_indices] vector of indices, each item being a set of indices representing a handle * @param[handle_coefficients] vector of cofficients, each item being the coefficients of the line representing the handle */ void HandleExtractor::extractHandles(PointCloud::Ptr &cloudInput, PointCloudNormal::Ptr &pointCloudNormals, std::vector< pcl::PointIndices> &handle_indices, std::vector< pcl::ModelCoefficients> &handle_coefficients ) { // setup handle cluster object pcl::EuclideanClusterExtraction<Point> handle_cluster_; KdTreePtr clusters_tree_(new KdTree); handle_cluster_.setClusterTolerance(handle_cluster_tolerance); handle_cluster_.setMinClusterSize(min_handle_cluster_size); handle_cluster_.setSearchMethod(clusters_tree_); handle_cluster_.setInputCloud(cloudInput); pcl::PointCloud<Point>::Ptr cloud_filtered(new pcl::PointCloud<Point>()); pcl::VoxelGrid<Point> vg; vg.setInputCloud(cloudInput); vg.setLeafSize(0.01, 0.01, 0.01); vg.filter(*cloud_filtered); // setup line ransac object pcl::SACSegmentation<Point> seg_line_; seg_line_.setModelType(pcl::SACMODEL_LINE); seg_line_.setMethodType(pcl::SAC_RANSAC); seg_line_.setInputCloud(cloudInput); seg_line_.setDistanceThreshold(line_ransac_distance); seg_line_.setOptimizeCoefficients(true); seg_line_.setMaxIterations(line_ransac_max_iter); // setup Inlier projection object pcl::ProjectInliers<Point> proj_; proj_.setInputCloud(cloud_filtered); proj_.setModelType(pcl::SACMODEL_PARALLEL_PLANE); // setup polygonal prism pcl::ExtractPolygonalPrismData<Point> prism_; prism_.setHeightLimits(min_handle_height, max_handle_height); prism_.setInputCloud(cloudInput); //setup Plane Segmentation outInfo("Segmenting planes"); pcl::SACSegmentation<Point> seg_plane_; seg_plane_.setOptimizeCoefficients(true); seg_plane_.setModelType(pcl::SACMODEL_PLANE); seg_plane_.setMethodType(pcl::SAC_RANSAC); seg_plane_.setDistanceThreshold(0.03); seg_plane_.setMaxIterations(500); seg_plane_.setInputCloud(cloud_filtered); pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients()); pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices()); seg_plane_.segment(*plane_inliers, *plane_coefficients); if(plane_inliers->indices.size() != 0) { outDebug("Plane model: " << plane_coefficients->values[0] << "," << plane_coefficients->values[1] << "," << plane_coefficients->values[2] << "," << plane_coefficients->values[3] << " with " << (int)plane_inliers->indices.size() << "inliers "); //Project inliers of the planes into a perfect plane PointCloud::Ptr cloud_projected(new PointCloud()); proj_.setIndices(plane_inliers); proj_.setModelCoefficients(plane_coefficients); proj_.filter(*cloud_projected); // Create a Convex Hull representation using the projected inliers PointCloud::Ptr cloud_hull(new PointCloud()); pcl::ConvexHull<Point> chull_; chull_.setDimension(2); chull_.setInputCloud(cloud_projected); chull_.reconstruct(*cloud_hull); outInfo("Convex hull has: " << (int)cloud_hull->points.size() << " data points."); if((int) cloud_hull->points.size() == 0) { outInfo("Convex hull has: no data points. Returning."); return; } // Extract the handle clusters using a polygonal prism pcl::PointIndices::Ptr handlesIndicesPtr(new pcl::PointIndices()); prism_.setInputPlanarHull(cloud_hull); prism_.segment(*handlesIndicesPtr); // cluster the points found in the prism std::vector< pcl::PointIndices> handle_clusters; handle_cluster_.setIndices(handlesIndicesPtr); handle_cluster_.extract(handle_clusters); for(size_t j = 0; j < handle_clusters.size(); j++) { // for each cluster in the prism, attempt to fit a line using ransac pcl::PointIndices single_handle_indices; pcl::ModelCoefficients handle_line_coefficients; seg_line_.setIndices(getIndicesPointerFromObject(handle_clusters[j])); seg_line_.segment(single_handle_indices, handle_line_coefficients); if(single_handle_indices.indices.size() > 0) { outInfo("Found a handle with " << single_handle_indices.indices.size() << " inliers."); outDebug("Handle Line coefficients: " << handle_line_coefficients); handle_indices.push_back(single_handle_indices); handle_coefficients.push_back(handle_line_coefficients); } } } else { outInfo("no planes found"); return; } }