// TODO: This method is only used for planar segmentation, consider removing Box3D minAreaRect(pcl::PointCloud<PointT>::Ptr &input_cloud) { //assuming chull is already sequential, if not we'll need to sort it. It seems like it is. Box3D box; pcl::PointCloud<PointT>::Ptr cloud_hull (new pcl::PointCloud<PointT>); pcl::ConvexHull<PointT> chull; chull.setInputCloud (input_cloud); chull.reconstruct (*cloud_hull); int n = cloud_hull->size(); PointT out[3]; if( n > 2 ) { rotatingCalipers( cloud_hull, n, CALIPERS_MINAREARECT, out ); box.center.x = out[0].x + (out[1].x + out[2].x)*0.5f; box.center.y = out[0].y + (out[1].y + out[2].y)*0.5f; /*box.size.xSize = (float)sqrt((double)out[1].x*out[1].x + (double)out[1].y*out[1].y); box.size.ySize = (float)sqrt((double)out[2].x*out[2].x + (double)out[2].y*out[2].y); box.angle = (float)atan2( (double)out[1].y, (double)out[1].x );*/ //xSize always the long! float norm1 = (float)sqrt((double)out[1].x*out[1].x + (double)out[1].y*out[1].y); float norm2 = (float)sqrt((double)out[2].x*out[2].x + (double)out[2].y*out[2].y); if(norm1 > norm2) { box.size.xSize = norm1; box.size.ySize = norm2; //box.angle = (float)atan2( (double)out[1].y, (double)out[1].x );//(float)atan(out[1].y/out[1].x);// } else { box.size.xSize = norm2; box.size.ySize = norm1; //box.angle = (float)atan2( (double)out[2].y, (double)out[2].x ); //(float)atan(out[2].y/out[2].x);// } } //haven't tested the below cases! else if( n == 2 ) { box.center.x = ( cloud_hull->points[0].x + cloud_hull->points[1].x)*0.5f; box.center.y = ( cloud_hull->points[0].y + cloud_hull->points[1].y)*0.5f; double dx = cloud_hull->points[1].x - cloud_hull->points[0].x; double dy = cloud_hull->points[1].y - cloud_hull->points[0].y; box.size.xSize = (float)sqrt(dx*dx + dy*dy); box.size.ySize = 0; //box.angle = (float)atan2( dy, dx ); } else { if( n == 1 ) box.center.x = cloud_hull->points[0].x; box.center.y = cloud_hull->points[0].y; //box.angle = 0; } //box.fillQuatGivenAxisAngle(); return box; }
int main (int argc, char** argv) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_projected (new pcl::PointCloud<pcl::PointXYZ>); pcl::PCDReader reader; reader.read ("table_scene_mug_stereo_textured.pcd", *cloud); // Build a filter to remove spurious NaNs pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (cloud); pass.setFilterFieldName ("z"); pass.setFilterLimits (0, 1.1); pass.filter (*cloud_filtered); std::cerr << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers (new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setDistanceThreshold (0.01); seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); std::cerr << "PointCloud after segmentation has: " << inliers->indices.size () << " inliers." << std::endl; // Project the model inliers pcl::ProjectInliers<pcl::PointXYZ> proj; proj.setModelType (pcl::SACMODEL_PLANE); proj.setIndices (inliers); proj.setInputCloud (cloud_filtered); proj.setModelCoefficients (coefficients); proj.filter (*cloud_projected); std::cerr << "PointCloud after projection has: " << cloud_projected->points.size () << " data points." << std::endl; // Create a Concave Hull representation of the projected inliers pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>); pcl::ConcaveHull<pcl::PointXYZ> chull; chull.setInputCloud (cloud_projected); chull.setAlpha (0.1); chull.reconstruct (*cloud_hull); std::cerr << "Concave hull has: " << cloud_hull->points.size () << " data points." << std::endl; pcl::PCDWriter writer; writer.write ("table_scene_mug_stereo_textured_hull.pcd", *cloud_hull, false); return (0); }
void DynModelExporter2::createMarkerForConvexHull(pcl::PointCloud<pcl::PointXYZ>& plane_cloud, pcl::ModelCoefficients::Ptr& plane_coefficients, visualization_msgs::Marker& marker) { // init marker marker.type = visualization_msgs::Marker::TRIANGLE_LIST; marker.action = visualization_msgs::Marker::ADD; // project the points of the plane on the plane pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::ProjectInliers<pcl::PointXYZ> proj; proj.setModelType (pcl::SACMODEL_PLANE); proj.setInputCloud (plane_cloud.makeShared()); proj.setModelCoefficients (plane_coefficients); proj.filter(*cloud_projected); // create the convex hull in the plane pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ> ()); pcl::ConvexHull<pcl::PointXYZ > chull; chull.setInputCloud (cloud_projected); chull.reconstruct(*cloud_hull); // work around known bug in ROS Diamondback perception_pcl: convex hull is centered around centroid of input cloud (fixed in pcl svn revision 443) // thus: we shift the mean of cloud_hull to the mean of cloud_projected (fill dx, dy, dz and apply when creating the marker points) Eigen::Vector4f meanPointCH, meanPointCP; pcl::compute3DCentroid(*cloud_projected, meanPointCP); pcl::compute3DCentroid(*cloud_hull, meanPointCH); //float dx = 0;//meanPointCP[0]-meanPointCH[0]; //float dy = 0;//meanPointCP[1]-meanPointCH[1]; //float dz = 0;//meanPointCP[2]-meanPointCH[2]; // create colored part of plane by creating marker for each triangle between neighbored points on contour of convex hull an midpoint marker.points.clear(); for (unsigned int j = 0; j < cloud_hull->points.size(); ++j) { geometry_msgs::Point p; p.x = cloud_hull->points[j].x; p.y = cloud_hull->points[j].y; p.z = cloud_hull->points[j].z; marker.points.push_back( p ); p.x = cloud_hull->points[(j+1)%cloud_hull->points.size() ].x; p.y = cloud_hull->points[(j+1)%cloud_hull->points.size()].y; p.z = cloud_hull->points[(j+1)%cloud_hull->points.size()].z; marker.points.push_back( p ); p.x = meanPointCP[0]; p.y = meanPointCP[1]; p.z = meanPointCP[2]; marker.points.push_back( p ); } // scale of the marker marker.scale.x = 1; marker.scale.y = 1; marker.scale.z = 1; }
visualization_msgs::Marker PlaneExt::AddPlanePoints(pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud) { // Add new plane hull pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>()); tVertices polygons = ComputeConcaveHull(plane_cloud, cloud_hull); // Join with current ConcaveHullJoinCurrent(cloud_hull, polygons); // Triangulate TriangulatePlanePolygon(); return planeTriangles; }
visualization_msgs::Marker PlaneExt::NewPlanePoints(pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud) { // Init a new plane hull (previous will be deleted pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>()); tVertices polygons = ComputeConcaveHull(plane_cloud, cloud_hull); // revrite hull ConcaveHullRewrite(cloud_hull, polygons); // triangulate TriangulatePlanePolygon(); return planeTriangles; }
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr calc_convex_hull(pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud, pcl::PointIndices::Ptr inliers, pcl::ModelCoefficients::Ptr coefficients){ pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZRGBA>), cloud_projected (new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::ConvexHull<pcl::PointXYZRGBA> chull; // Project the model inliers pcl::ProjectInliers<pcl::PointXYZRGBA> proj; proj.setModelType (pcl::SACMODEL_PLANE); proj.setIndices (inliers); proj.setInputCloud (cloud); proj.setModelCoefficients (coefficients); proj.filter (*cloud_projected); // Create a Convex Hull representation of the projected inliers chull.setInputCloud (cloud_projected); //chull.setAlpha (0.1); chull.reconstruct (*cloud_hull); return cloud_hull; }
//extract table clouds, convex hull, and convert to msg stored in DB bool extract_table_msg(pcl_cloud::Ptr cloud_in, bool is_whole, bool store_cloud=false, bool store_convex=false) { pcl_cloud::Ptr cloud1(new pcl_cloud()); pcl_cloud::Ptr cloud_out(new pcl_cloud()); //filter out range that may not be a table plane pcl::PassThrough<Point> pass; pass.setFilterFieldName("z"); pass.setFilterLimits(0.45,1.2); pass.setInputCloud(cloud_in); pass.filter(*cloud1); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::SACSegmentation<Point> seg; seg.setOptimizeCoefficients (true); //plane model seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setDistanceThreshold (0.01); seg.setInputCloud (cloud1); seg.segment (*inliers, *coefficients); if(inliers->indices.size () == 0){ ROS_INFO("No plane detected!"); return false; } //form a new cloud from indices pcl::ExtractIndices<Point> extract; extract.setInputCloud (cloud1); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloud_out); //filter out noise pcl_cloud::Ptr cloud_nonoise(new pcl_cloud()); outlier_filter_radius(cloud_out,cloud_nonoise); /* //normal estimation and filter table plane pcl::NormalEstimationOMP<Point, pcl::Normal> ne; ne.setInputCloud (cloud_nonoise); pcl::search::KdTree<Point>::Ptr tree (new pcl::search::KdTree<Point> ()); ne.setSearchMethod (tree); pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); ne.setRadiusSearch (0.03); ne.compute (*cloud_normals); pcl::Normal nor; float size = 0.0; for(int i=0;i<cloud_normals->height*cloud_normals->width;i++){ if(isnan(cloud_normals->at(i).normal_x)){ //std::cout<<cloud_normals->at(i).normal_x<<std::endl; } else{ nor.normal_x += cloud_normals->at(i).normal_x; nor.normal_y += cloud_normals->at(i).normal_y; nor.normal_z += cloud_normals->at(i).normal_z; size++; } } //normalize instead of average float normal_length = sqrt(nor.normal_x*nor.normal_x+nor.normal_y*nor.normal_y+nor.normal_z*nor.normal_z); nor.normal_x = nor.normal_x/normal_length; nor.normal_y = nor.normal_y/normal_length; nor.normal_z = nor.normal_z/normal_length; */ //instead of computer normal again, using it from plane model pcl::Normal nor; nor.normal_x = coefficients->values[0]; nor.normal_y = coefficients->values[1]; nor.normal_z = coefficients->values[2]; if(Debug){ std::cout<<nor.normal_x<<std::endl; std::cout<<nor.normal_y<<std::endl; std::cout<<nor.normal_z<<std::endl; } //the default viewpoint is (0,0,0),thus using global cloud some plane and viewpoint are in a same plane, //which may not helpful to flip normal direction pcl::Normal standard_normal; standard_normal.normal_x = 0.0; standard_normal.normal_y = 0.0; standard_normal.normal_z = 1.0; float cosin_angle = (standard_normal.normal_z*nor.normal_z); float tmp_angle = acos(cosin_angle) * (180.0/PI); //if(tmp_angle>90.0?(tmp_angle-90.0)<normal_angle:tmp_angle<normal_angle){ if(tmp_angle-90.0>0.0){ if((180.0 - tmp_angle)<normal_angle){ ROS_INFO("Normal direction meet requirement %f, angle calculated is: 180.0-%f=%f. ", normal_angle, tmp_angle, 180.0-tmp_angle); } else{ ROS_INFO("Normal direction exceed threshold %f, angle calculated is: 180.0-%f=%f. skip.", normal_angle, tmp_angle, 180.0-tmp_angle); return false; } } else{ if(tmp_angle<normal_angle) { ROS_INFO("Normal direction meet requirement angle %f, angle calculated is %f", normal_angle, tmp_angle); } else{ ROS_INFO("Normal direction exceed threshold %f, angle calculated is: %f. skip.", normal_angle, tmp_angle); return false; } } pcl_cloud::Ptr cloud_hull(new pcl_cloud()); pcl_cloud::Ptr cloud_cave(new pcl_cloud()); extract_convex(cloud1,inliers,coefficients,cloud_hull,cloud_cave); //reject some size convex //store the plane that pass the table filter if(store_cloud) { ROS_INFO("Storing table cloud (noise & filted)..."); if (is_whole) { mongodb_store::MessageStoreProxy table_whole_cloud_noise(*nh, "whole_table_clouds_noise"); mongodb_store::MessageStoreProxy table_whole_cloud(*nh, "whole_table_clouds"); //conversion sensor_msgs::PointCloud2 whole_table_cloud_noise; pcl::toROSMsg(*cloud_out, whole_table_cloud_noise); table_whole_cloud_noise.insert(whole_table_cloud_noise); sensor_msgs::PointCloud2 whole_table_cloud; pcl::toROSMsg(*cloud_nonoise, whole_table_cloud); table_whole_cloud.insert(whole_table_cloud); } else { mongodb_store::MessageStoreProxy dbtable_cloud_noise(*nh, "table_clouds_noise"); mongodb_store::MessageStoreProxy dbtable_cloud(*nh, "table_clouds"); //conversion sensor_msgs::PointCloud2 table_cloud_noise; pcl::toROSMsg(*cloud_out, table_cloud_noise); dbtable_cloud_noise.insert(table_cloud_noise); sensor_msgs::PointCloud2 table_cloud; pcl::toROSMsg(*cloud_nonoise, table_cloud); dbtable_cloud.insert(table_cloud); //store table centre for each scan ROS_INFO("Storing table centre..."); Table<Point> tb(nh); tb.table_cloud_centre(table_cloud); } } ROS_INFO("Convex cloud has been extracted contains %lu points", (*cloud_hull).points.size()); if(store_convex){ ROS_INFO("Storing convex cloud..."); if(is_whole){ mongodb_store::MessageStoreProxy dbtable_whole_convex_cloud(*nh, "whole_table_convex"); mongodb_store::MessageStoreProxy dbtable_whole_concave_cloud(*nh, "whole_table_concave"); //conversion sensor_msgs::PointCloud2 whole_table_convex; pcl::toROSMsg(*cloud_hull, whole_table_convex); dbtable_whole_convex_cloud.insert(whole_table_convex); sensor_msgs::PointCloud2 whole_table_concave; pcl::toROSMsg(*cloud_cave, whole_table_concave); dbtable_whole_concave_cloud.insert(whole_table_concave); } else{ mongodb_store::MessageStoreProxy dbtable_convex_cloud(*nh, "table_convex"); mongodb_store::MessageStoreProxy dbtable_concave_cloud(*nh, "table_concave"); //conversion sensor_msgs::PointCloud2 table_convex; pcl::toROSMsg(*cloud_hull, table_convex); dbtable_convex_cloud.insert(table_convex); sensor_msgs::PointCloud2 table_concave; pcl::toROSMsg(*cloud_cave, table_concave); dbtable_concave_cloud.insert(table_concave); } } //construct a table msg table_detection::Table table_msg; table_msg.pose.pose.orientation.w = 1.0; table_msg.header.frame_id = "/map"; table_msg.header.stamp = ros::Time(); for(int i=0;i<(*cloud_hull).points.size();i++){ geometry_msgs::Point32 pp; pp.x = (*cloud_hull).at(i).x; pp.y = (*cloud_hull).at(i).y; pp.z = (*cloud_hull).at(i).z; table_msg.tabletop.points.push_back(pp); } //insert to mongodb if(is_whole){ mongodb_store::MessageStoreProxy whole_table_shape(*nh, "whole_table_shapes"); whole_table_shape.insert(table_msg); ROS_INFO("Insert whole table shape to collection."); } else{ mongodb_store::MessageStoreProxy table_shape(*nh, "table_shapes"); table_shape.insert(table_msg); ROS_INFO("Insert table shape to collection."); } return true; }
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; }
void FilterPlanes::filterPlanes(vector<BasicPlane> &plane_stack){ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>); if (verbose_text>0){ cout << "PointCloud before filtering: " << cloud->width * cloud->height << " data points." << std::endl; } #if DO_TIMING_PROFILE vector<int64_t> tic_toc; tic_toc.push_back(bot_timestamp_now()); #endif /* Ptcoll_cfg ptcoll_cfg; ptcoll_cfg.point_lists_id =pose_element_id; //bot_timestamp_now(); ptcoll_cfg.collection = pose_coll_id; ptcoll_cfg.element_id = pose_element_id; */ //1. Downsample the dataset using a leaf size of 1cm // this is 99% of the cpu time pcl::VoxelGrid<pcl::PointXYZRGB> sor; sor.setInputCloud (cloud); // for table dataset: sor.setLeafSize (0.01, 0.01, 0.01); sor.setLeafSize (0.05, 0.05, 0.05); // sor.setLeafSize (0.1, 0.1, 0.1); sor.filter (*cloud_filtered); if (verbose_text>0){ cout << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl; } #if DO_TIMING_PROFILE tic_toc.push_back(bot_timestamp_now()); #endif if (verbose_lcm>2){ /* ptcoll_cfg.id = 200; ptcoll_cfg.reset=true; ptcoll_cfg.name ="cloud_downsampled"; ptcoll_cfg.npoints = cloud_filtered->points.size(); float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0}; ptcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float)); ptcoll_cfg.type =1; pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg, *cloud_filtered);*/ } // 2. Set up the Ransac Plane Fitting Object: pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); pcl::SACSegmentation<pcl::PointXYZRGB> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); // was 4000 seg.setDistanceThreshold (distance_threshold_); // 0.01 for table data set // Create the filtering object pcl::ExtractIndices<pcl::PointXYZRGB> extract; #if DO_TIMING_PROFILE tic_toc.push_back(bot_timestamp_now()); #endif int n_major = 0, nr_points = cloud_filtered->points.size (); //vector<BasicPlaneX> plane_stack; BasicPlane one_plane; // Extract the primary planes: // major planes are the coarse planes extracted via ransac. they might contain disconnected points // these are broken up into minor planes which are portions of major planes if(verbose_lcm > 2){ for (int i=0;i<7;i++){ char n_major_char[10]; sprintf(n_major_char,"%d",i); /* ptcoll_cfg.id =210+ i+3; ptcoll_cfg.reset=true; ptcoll_cfg.name =(char*) "cloud_p "; ptcoll_cfg.name.append(n_major_char); ptcoll_cfg.npoints = 0; float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0}; ptcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float)); ptcoll_cfg.type=1; pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg, *cloud_filtered); */ } for (int i=0;i<7;i++){ /* Ptcoll_cfg ptcoll_cfg2; // the i below accounts for super planes in the same utime ptcoll_cfg2.point_lists_id =pose_element_id; //bot_timestamp_now(); ptcoll_cfg2.collection = pose_coll_id; ptcoll_cfg2.element_id = pose_element_id; if (i==0){ ptcoll_cfg2.reset=true; }else{ ptcoll_cfg2.reset=false; } ptcoll_cfg2.id =500; ptcoll_cfg2.name.assign("Grown Stack Final"); ptcoll_cfg2.npoints = 0; ptcoll_cfg2.type =1; pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg2, *cloud_filtered); */ } // TODO: this will rest this object. need to add publish of reset // at start of this block and set rese ttofal for (int i=0;i<7;i++){ /* Ptcoll_cfg ptcoll_cfg2; // the i below accounts for super planes in the same utime ptcoll_cfg2.point_lists_id =pose_element_id; //bot_timestamp_now(); ptcoll_cfg2.collection = pose_coll_id; ptcoll_cfg2.element_id = pose_element_id; if (i==0){ ptcoll_cfg2.reset=true; }else{ ptcoll_cfg2.reset=false; } ptcoll_cfg2.id =501; ptcoll_cfg2.name.assign("Grown Stack Final Hull"); ptcoll_cfg2.npoints = 0; ptcoll_cfg2.type =3; pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg2, *cloud_filtered); */ } } #if DO_TIMING_PROFILE tic_toc.push_back(bot_timestamp_now()); #endif while (cloud_filtered->points.size () > stop_proportion_ * nr_points) { // While XX% of the original cloud is still there char n_major_char[10]; sprintf(n_major_char,"%d",n_major); if (n_major >6) { if (verbose_text >0){ std::cout << n_major << " is the max number of planes to find, quitting" << std::endl; } break; } //a Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cout << "Could not estimate a planar model for the given dataset." << std::endl; break; } if (inliers->indices.size () < stop_cloud_size_) // stop when the plane is only a few points { //std::cerr << "No remaining planes in this set" << std::endl; break; } //b Extract the inliers extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloud_p); if (verbose_text>0){ std::cout << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl; } // Create the filtering object pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor; sor.setInputCloud (cloud_p); sor.setMeanK (30); sor.setStddevMulThresh (0.5); //was 1.0 sor.filter (*cloud_p); if(verbose_lcm > 2){ /* ptcoll_cfg.id =210+ n_major+3; ptcoll_cfg.reset=true; ptcoll_cfg.name ="cloud_p "; ptcoll_cfg.name.append(n_major_char); ptcoll_cfg.npoints = cloud_p->points.size(); float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0}; ptcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float)); ptcoll_cfg.type=1; pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg, *cloud_p); */ } vector<BasicPlane> grown_stack; vector<BasicPlane> * grown_stack_ptr; grown_stack_ptr = &grown_stack; GrowCloud grow; grow.setInputCloud(cloud_p); grow.setMinCloudSize(stop_cloud_size_); // useing stop cloud size here too grow.setLCM(publish_lcm); grow.doGrowCloud(*grown_stack_ptr); if (verbose_text>0){ cout << "grow_cloud new found " << grown_stack.size() << " seperate clouds\n"; } // Spit raw clouds out to LCM: if(verbose_lcm > 2){ for (int i=0;i<grown_stack.size();i++){ /* Ptcoll_cfg ptcoll_cfg2; ptcoll_cfg2.reset=false; // the i below accounts for super planes in the same utime ptcoll_cfg2.point_lists_id =10*n_major + i; //filterplanes->pose_element_id; ptcoll_cfg2.collection = pose_coll_id; ptcoll_cfg2.element_id = pose_element_id; ptcoll_cfg2.id =500; ptcoll_cfg2.name.assign("Grown Stack Final"); ptcoll_cfg2.npoints = grown_stack[i].cloud.points.size (); ptcoll_cfg2.type =1; int id = plane_stack.size() + i; // 10*n_major + i; float colorm_temp0[] ={colors[3*(id%num_colors)],colors[3*(id%num_colors)+1],colors[3*(id%num_colors)+2] ,0.0}; ptcoll_cfg2.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float)); pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg2, grown_stack[i].cloud); */ } } BasicPlane one_plane; int n_minor=0; BOOST_FOREACH( BasicPlane one_plane, grown_stack ){ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZRGB>); // c Project the model inliers (seems to be necessary to fitting convex hull pcl::ProjectInliers<pcl::PointXYZRGB> proj; proj.setModelType (pcl::SACMODEL_PLANE); pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp (new pcl::PointCloud<pcl::PointXYZRGB> ()); *temp = (one_plane.cloud); proj.setInputCloud (temp); proj.setModelCoefficients (coefficients); proj.filter (*cloud_projected); std::vector <pcl::Vertices> vertices; if (1==1){ // convex: pcl::ConvexHull<pcl::PointXYZRGB> chull; chull.setInputCloud (cloud_projected); chull.setDimension(2); chull.reconstruct (*cloud_hull,vertices); }else { // concave pcl::ConcaveHull<pcl::PointXYZRGB> chull; chull.setInputCloud (cloud_projected); chull.setKeepInformation(true); chull.setAlpha(0.5); // for arch way: // 1.1 too few // 0.7 a little to few but much better chull.reconstruct (*cloud_hull,vertices); } //std::cout << "Hull has: " << cloud_hull->points.size () << " vertices." << std::endl; if (cloud_hull->points.size () ==0){ cout <<"ERROR: CONVEX HULL HAS NO POINTS! - NEED TO RESOLVE THIS\n"; } // d.2 Find the mean colour of the hull: int rgba[]={0,0,0,0}; for(int i=0;i<temp->points.size();i++){ int rgba_one = *reinterpret_cast<int*>(&temp->points[i].rgba); rgba[3] += ((rgba_one >> 24) & 0xff); rgba[2] += ((rgba_one >> 16) & 0xff); rgba[1] += ((rgba_one >> 8) & 0xff); rgba[0] += (rgba_one & 0xff); } double scale = ((double) temp->points.size()); rgba[3] =(int) round(((double) rgba[3]) / scale); rgba[2] =(int) round(((double) rgba[2]) / scale); rgba[1] =(int) round(((double) rgba[1]) / scale); rgba[0] =(int) round(((double) rgba[0]) / scale); // Write the plane to file for experimentation: //stringstream oss; //oss << ptcoll_cfg.element_id <<"_" << ptcoll_cfg.name << ".pcd"; //writer.write (oss.str(), *this_hull, false); for(int i=0;i<cloud_hull->points.size();i++){ unsigned char* rgba_ptr = (unsigned char*)&cloud_hull->points[i].rgba; (*rgba_ptr) = rgba[0] ; (*(rgba_ptr+1)) = rgba[1]; (*(rgba_ptr+2)) = rgba[2]; (*(rgba_ptr+3)) = rgba[3]; } (one_plane.coeffs) = *coefficients; one_plane.cloud = *cloud_hull; one_plane.utime = pose_element_id; one_plane.major = n_major; one_plane.minor = n_minor; one_plane.n_source_points = cloud_projected->points.size(); plane_stack.push_back(one_plane); n_minor++; // int stop; // cout << "int to cont: "; // cin >> stop; } // Create the filtering object extract.setNegative (true); extract.filter (*cloud_filtered); n_major++; }
int main (int argc, char** argv) { // ros::init(argc, argv, "extract_sec"); // ros::NodeHandle node_handle; // pcl::visualization::PCLVisualizer result_viewer("planar_segmentation"); boost::shared_ptr<pcl::visualization::PCLVisualizer> result_viewer (new pcl::visualization::PCLVisualizer ("planar_segmentation")); result_viewer->addCoordinateSystem(0.3, "reference", 0); result_viewer->setCameraPosition(-0.499437, 0.111597, -0.758007, -0.443141, 0.0788583, -0.502855, -0.034703, -0.992209, -0.119654); result_viewer->setCameraClipDistances(0.739005, 2.81526); // result_viewer->setCameraPosition(Position, Focal point, View up); // result_viewer->setCameraClipDistances(Clipping plane); /*************************************** * parse arguments ***************************************/ if(argc<5) { pcl::console::print_info("Usage: extract_sec DATA_PATH/PCD_FILE_FORMAT START_INDEX END_INDEX DEMO_NAME (opt)STEP_SIZE(1)"); exit(1); } int view_id=0; int step=1; std::string basename_cloud=argv[1]; unsigned int index_start = std::atoi(argv[2]); unsigned int index_end = std::atoi(argv[3]); std::string demo_name=argv[4]; if(argc>5) step=std::atoi(argv[5]); /*************************************** * set up result directory ***************************************/ mkdir("/home/zhen/Documents/Dataset/human_result", S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH); char result_folder[50]; std::snprintf(result_folder, sizeof(result_folder), "/home/zhen/Documents/Dataset/human_result/%s", demo_name.c_str()); mkdir(result_folder, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH); std::string basename_pcd = (basename_cloud.find(".pcd") == std::string::npos) ? (basename_cloud + ".pcd") : basename_cloud; std::string filename_pcd; std::string mainGraph_file; mainGraph_file = std::string(result_folder) + "/mainGraph.txt"; // write video config char video_file[100]; std::snprintf(video_file, sizeof(video_file), "%s/video.txt", result_folder); std::ofstream video_config(video_file); if (video_config.is_open()) { video_config << index_start << " " << index_end << " " << demo_name << " " << step; video_config.close(); } /*************************************** * set up cloud, segmentation, tracker, detectors, graph, features ***************************************/ TableObject::Segmentation tableObjSeg; TableObject::Segmentation initialSeg; TableObject::track3D tracker(false); TableObject::colorDetector finger1Detector(0,100,0,100,100,200); //right TableObject::colorDetector finger2Detector(150,250,0,100,0,100); //left TableObject::touchDetector touchDetector(0.01); TableObject::bottleDetector bottleDetector; TableObject::mainGraph mainGraph((int)index_start); std::vector<manipulation_features> record_features; manipulation_features cur_features; TableObject::pcdCloud pcdSceneCloud; CloudPtr sceneCloud; CloudPtr planeCloud(new Cloud); CloudPtr cloud_objects(new Cloud); CloudPtr cloud_finger1(new Cloud); CloudPtr cloud_finger2(new Cloud); CloudPtr cloud_hull(new Cloud); CloudPtr track_target(new Cloud); CloudPtr tracked_cloud(new Cloud); std::vector<pcl::PointIndices> clusters; pcl::ModelCoefficients coefficients; pcl::PointIndices f1_indices; pcl::PointIndices f2_indices; Eigen::Affine3f toBottleCoordinate; Eigen::Affine3f transformation; Eigen::Vector3f bottle_init_ori; // set threshold of size of clustered cloud tableObjSeg.setThreshold(30); initialSeg.setThreshold(500); // downsampler pcl::ApproximateVoxelGrid<pcl::PointXYZRGBA> grid; float leaf_size=0.005; grid.setLeafSize (leaf_size, leaf_size, leaf_size); /*************************************** * start processing ***************************************/ unsigned int idx = index_start; int video_id=0; bool change = false; while( idx <= index_end && !result_viewer->wasStopped()) { std::cout << std::endl; std::cout << "frame id=" << idx << std::endl; filename_pcd = cv::format(basename_cloud.c_str(), idx); if(idx==index_start) { /*************************************** * Intialization: * -plane localization * -object cluster extraction * -bottle cluster localization ***************************************/ initialSeg.resetCloud(filename_pcd, false); initialSeg.seg(false); initialSeg.getObjects(cloud_objects, clusters); initialSeg.getCloudHull(cloud_hull); initialSeg.getPlaneCoefficients(coefficients); initialSeg.getsceneCloud(pcdSceneCloud); initialSeg.getTableTopCloud(planeCloud); sceneCloud=pcdSceneCloud.getCloud(); /*************************************** * fingertip, hand_arm removal ***************************************/ //opencv color filtering for fingertip_1 { pcl::ScopeTime t_finger1("Finger 1(blue) detection"); finger1Detector.setInputCloud(cloud_objects, clusters); finger1Detector.filter(f1_indices,cloud_finger1); } finger1Detector.showDetectedCloud(result_viewer, "finger1"); //opencv color filtering for fingertip_2 { pcl::ScopeTime t_finger2("Finger 2(orange) detection"); finger2Detector.setInputCloud(cloud_objects, clusters); finger2Detector.filter(f2_indices,cloud_finger2); } finger2Detector.showDetectedCloud(result_viewer, "finger2"); // remove hand (include cluster that contains the detected fingertips and also the other clusters that are touching the cluster) std::vector<int> hand_arm1=TableObject::findHand(cloud_objects, clusters, f1_indices); for(int i=hand_arm1.size()-1; i>=0; i--) { clusters.erase(clusters.begin()+hand_arm1[i]); std::cout << "removing hand_arm : cluster index = " << hand_arm1[i] << std::endl; } std::vector<int> hand_arm2=TableObject::findHand(cloud_objects, clusters, f2_indices); for(int i=hand_arm2.size()-1; i>=0; i--) { clusters.erase(clusters.begin()+hand_arm2[i]); std::cout << "removing hand_arm : cluster index = " << hand_arm2[i] << std::endl; } // DEBUG // pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> plane(planeCloud); // result_viewer->addPointCloud<RefPointType>(planeCloud, plane, "tabletop"); // CloudPtr debug(new Cloud); // initialSeg.getOutPlaneCloud(debug); // pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> out_plane(debug); // result_viewer->addPointCloud<RefPointType>(debug, out_plane, "out_plane"); // choose bottle_id at 1st frame & confirm fitted model is correct TableObject::view3D::drawClusters(result_viewer, cloud_objects, clusters, true); while (!result_viewer->wasStopped ()) { result_viewer->spinOnce (100); boost::this_thread::sleep (boost::posix_time::microseconds (100000)); } std::cout << "cluster size = " << clusters.size() << std::endl; /*************************************** * Localizing cylinder ***************************************/ CloudPtr cluster_bottle (new Cloud); int bottle_id = 0; pcl::copyPointCloud (*cloud_objects, clusters[bottle_id], *cluster_bottle); bottleDetector.setInputCloud(cluster_bottle); bottleDetector.fit(); bottleDetector.getTransformation(toBottleCoordinate); bottle_init_ori= bottleDetector.getOrientation(); float x, y, z, roll, pitch, yaw; pcl::getTranslationAndEulerAngles(toBottleCoordinate.inverse(), x, y, z, roll, pitch, yaw); result_viewer->removeCoordinateSystem("reference", 0); result_viewer->addCoordinateSystem(0.3, toBottleCoordinate.inverse(), "reference", 0); bottleDetector.drawOrientation(result_viewer); /*************************************** * Record features ***************************************/ bottle_features cur_bottle_features; cur_bottle_features.loc[0] = x; cur_bottle_features.loc[1] = y; cur_bottle_features.loc[2] = z; cur_bottle_features.ori[0] = roll; cur_bottle_features.ori[1] = pitch; cur_bottle_features.ori[2] = yaw; cur_bottle_features.color[0] = bottleDetector.getCenter().r; cur_bottle_features.color[1] = bottleDetector.getCenter().g; cur_bottle_features.color[2] = bottleDetector.getCenter().b; cur_bottle_features.size[0] = bottleDetector.getHeight(); cur_bottle_features.size[1] = bottleDetector.getRadius(); cur_features.bottle = cur_bottle_features; pcl::PointXYZ center_finger1 = TableObject::computeObjCentroid(cloud_finger1); pcl::PointXYZ center_finger2 = TableObject::computeObjCentroid(cloud_finger2); center_finger1 = pcl::transformPoint<pcl::PointXYZ>(center_finger1, toBottleCoordinate); center_finger2 = pcl::transformPoint<pcl::PointXYZ>(center_finger2, toBottleCoordinate); cur_features.gripper_1.loc[0] = center_finger1.x; cur_features.gripper_1.loc[1] = center_finger1.y; cur_features.gripper_1.loc[2] = center_finger1.z; cur_features.gripper_2.loc[0] = center_finger2.x; cur_features.gripper_2.loc[1] = center_finger2.y; cur_features.gripper_2.loc[2] = center_finger2.z; record_features.push_back(cur_features); /*************************************** * Tracking initialization ***************************************/ { pcl::ScopeTime t_track("Tracker initialization"); tracker.setTarget(cluster_bottle, bottleDetector.getCenter()); tracker.initialize(); } /*************************************** * Touch detection ***************************************/ std::vector<CloudPtr> touch_clouds; touch_clouds.push_back(cluster_bottle); touch_clouds.push_back(cloud_finger1); touch_clouds.push_back(cloud_finger2); // touch detection between each pair of objects (including fingertips, tabletop objects and tabletop) for(int i=0; i<touch_clouds.size(); i++) { int j; bool touch; for(j=i+1; j<touch_clouds.size(); j++) { // touch detection between object_i and object_j char relation [50]; std::sprintf(relation, "object%d_object%d", i, j); std::cout << relation << std::endl; { pcl::ScopeTime t("Touch detection"); touch=touchDetector.detect(touch_clouds[i], touch_clouds[j]); } // touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i); // relational scene graph -> main graph if(touch) { mainGraph.addInitialRelationalGraph(2); } else { mainGraph.addInitialRelationalGraph(0); } } // touch detection between each objects and tabletop char relation [50]; std::sprintf (relation, "object%d_object%d", i, (int)touch_clouds.size()); std::cout << relation << std::endl; { pcl::ScopeTime t("Touch detection"); touch=touchDetector.detectTableTouch(touch_clouds[i], coefficients); } // touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i); // relational scene graph -> main graph if(touch) { mainGraph.addInitialRelationalGraph(2); } else { mainGraph.addInitialRelationalGraph(0); } } /*************************************** * Visualization ***************************************/ // draw extracted object clusters // TableObject::view3D::drawClusters(result_viewer, cloud_objects, touch_clusters); // draw extracted plane points // pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> plane(planeCloud); // result_viewer->addPointCloud<RefPointType>(planeCloud, plane, "tabletop"); // std::stringstream ss; // ss << (int)touch_clusters.size(); // result_viewer->addText3D(ss.str(), planeCloud->points.at(334*640+78),0.1); // draw extracted plane contour polygon result_viewer->addPolygon<RefPointType>(cloud_hull, 0, 255, 0, "polygon"); change = true; } else { /*************************************** * object cloud extraction ***************************************/ tableObjSeg.resetCloud(filename_pcd, false); tableObjSeg.seg(cloud_hull,false); tableObjSeg.getObjects(cloud_objects, clusters); tableObjSeg.getsceneCloud(pcdSceneCloud); sceneCloud=pcdSceneCloud.getCloud(); /*************************************** * fingertip extraction ***************************************/ //opencv color filtering for fingertip_1 { pcl::ScopeTime t_finger1("Finger 1(blue) detection"); finger1Detector.setInputCloud(cloud_objects, clusters); finger1Detector.filter(f1_indices,cloud_finger1); } finger1Detector.showDetectedCloud(result_viewer, "finger1"); //opencv color filtering for fingertip_2 { pcl::ScopeTime t_finger1("Finger 2(orange) detection"); finger2Detector.setInputCloud(cloud_objects, clusters); finger2Detector.filter(f2_indices,cloud_finger2); } finger2Detector.showDetectedCloud(result_viewer, "finger2"); /*************************************** * filter out black glove cluster & gray sleeve, also update cloud_objects with removed cluster indices ***************************************/ for(int i=0; i<clusters.size(); i++) { pcl::CentroidPoint<RefPointType> color_points; for(int j=0; j<clusters[i].indices.size(); j++) { color_points.add(cloud_objects->at(clusters[i].indices[j])); } RefPointType mean_color; color_points.get(mean_color); if(mean_color.r>30 & mean_color.r<70 & mean_color.g>30 & mean_color.g<70 & mean_color.b>30 & mean_color.b<70) { clusters.erase(clusters.begin()+ i); i=i-1; } } /*************************************** * Tracking objects ***************************************/ { pcl::ScopeTime t_track("Tracking"); grid.setInputCloud (sceneCloud); grid.filter (*track_target); tracker.track(track_target, transformation); tracker.getTrackedCloud(tracked_cloud); } tracker.viewTrackedCloud(result_viewer); // tracker.drawParticles(result_viewer); /*************************************** * compute tracked <center, orientation> ***************************************/ pcl::PointXYZ bottle_loc_point(0,0,0); bottle_loc_point = pcl::transformPoint<pcl::PointXYZ>(bottle_loc_point, transformation); result_viewer->removeShape("bottle_center"); // result_viewer->addSphere<pcl::PointXYZ>(bottle_loc_point, 0.05, "bottle_center"); Eigen::Vector3f bottle_ori; pcl::transformVector(bottle_init_ori,bottle_ori,transformation); TableObject::view3D::drawArrow(result_viewer, bottle_loc_point, bottle_ori, "bottle_arrow"); /*************************************** * calculate toTrackedBottleCoordinate ***************************************/ Eigen::Affine3f toTrackedBottleCoordinate; Eigen::Vector3f p( bottle_loc_point.x, bottle_loc_point.y, bottle_loc_point.z ); // position // get a vector that is orthogonal to _orientation ( yc = _orientation x [1,0,0]' ) Eigen::Vector3f yc( 0, bottle_ori[2], -bottle_ori[1] ); yc.normalize(); // get a transform that rotates _orientation into z and moves cloud into origin. pcl::getTransformationFromTwoUnitVectorsAndOrigin(yc, bottle_ori, p, toTrackedBottleCoordinate); result_viewer->removeCoordinateSystem("reference"); result_viewer->addCoordinateSystem(0.3, toTrackedBottleCoordinate.inverse(), "reference", 0); float x, y, z, roll, pitch, yaw; pcl::getTranslationAndEulerAngles(toTrackedBottleCoordinate.inverse(), x, y, z, roll, pitch, yaw); /*************************************** * setup bottle feature ***************************************/ cur_features = record_features[video_id-1]; cur_features.bottle.loc[0] = x; cur_features.bottle.loc[1] = y; cur_features.bottle.loc[2] = z; cur_features.bottle.ori[0] = roll; cur_features.bottle.ori[1] = pitch; cur_features.bottle.ori[2] = yaw; pcl::PointXYZ center_finger1 = TableObject::computeObjCentroid(cloud_finger1); pcl::PointXYZ center_finger2 = TableObject::computeObjCentroid(cloud_finger2); center_finger1 = pcl::transformPoint<pcl::PointXYZ>(center_finger1, toTrackedBottleCoordinate); center_finger2 = pcl::transformPoint<pcl::PointXYZ>(center_finger2, toTrackedBottleCoordinate); cur_features.gripper_1.loc[0] = center_finger1.x; cur_features.gripper_1.loc[1] = center_finger1.y; cur_features.gripper_1.loc[2] = center_finger1.z; cur_features.gripper_2.loc[0] = center_finger2.x; cur_features.gripper_2.loc[1] = center_finger2.y; cur_features.gripper_2.loc[2] = center_finger2.z; record_features.push_back(cur_features); /*************************************** * Touch detection ***************************************/ std::vector<CloudPtr> touch_clouds; touch_clouds.push_back(tracked_cloud); touch_clouds.push_back(cloud_finger1); touch_clouds.push_back(cloud_finger2); // touch detection between each pair of objects (including fingertips, tabletop objects and tabletop) for(int i=0; i<touch_clouds.size(); i++) { int j; bool touch; for(j=i+1; j<touch_clouds.size(); j++) { // touch detection between object_i and object_j char relation [50]; std::sprintf(relation, "object%d_object%d", i, j); std::cout << relation << std::endl; { pcl::ScopeTime t("Touch detection"); touch=touchDetector.detect(touch_clouds[i], touch_clouds[j]); } // touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i); // relational scene graph -> main graph if(touch) { mainGraph.addRelationalGraph(2); } else { mainGraph.addRelationalGraph(0); } } // touch detection between each objects and tabletop char relation [50]; std::sprintf (relation, "object%d_object%d", i, (int)touch_clouds.size()); std::cout << relation << std::endl; { pcl::ScopeTime t("Touch detection"); touch=touchDetector.detectTableTouch(touch_clouds[i], coefficients); } // touchDetector.showTouch(result_viewer, relation, 100+250*(j-i-1), 40+20*i); // relational scene graph -> main graph if(touch) { mainGraph.addRelationalGraph(2); } else { mainGraph.addRelationalGraph(0); } } /*************************************** * Visualization ***************************************/ // draw extracted point clusters // TableObject::view3D::drawText(result_viewer, cloud_objects, touch_clusters); /*************************************** * Main Graph ***************************************/ change = mainGraph.compareRelationGraph((int)idx); } // darw original cloud pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> rgb(sceneCloud); if(!result_viewer->updatePointCloud<RefPointType>(sceneCloud, rgb, "new frame")) result_viewer->addPointCloud<RefPointType>(sceneCloud, rgb, "new frame"); result_viewer->spinOnce (100); boost::this_thread::sleep (boost::posix_time::microseconds (100000)); //debug std::cout << cur_features.bottle.loc[0] << " " << cur_features.bottle.loc[1] << " " << cur_features.bottle.loc[2] << " " << cur_features.bottle.ori[0] << " " << cur_features.bottle.ori[1] << " " << cur_features.bottle.ori[2] << " " << cur_features.bottle.color[0] << " " << cur_features.bottle.color[1] << " " << cur_features.bottle.color[2] << " " << cur_features.bottle.size[0] << " " << cur_features.bottle.size[1] << " " << cur_features.gripper_1.loc[0] << " " << cur_features.gripper_1.loc[1] << " " << cur_features.gripper_1.loc[2] << " " << cur_features.gripper_2.loc[0] << " " << cur_features.gripper_2.loc[1] << " " << cur_features.gripper_2.loc[2] << std::endl; if(change) { char screenshot[100]; // make sure it's big enough std::snprintf(screenshot, sizeof(screenshot), "%s/sec_%d.png", result_folder, (int)idx); std::cout << screenshot << std::endl; result_viewer->saveScreenshot(screenshot); //record features char feature_file[100]; // make sure it's big enough std::snprintf(feature_file, sizeof(feature_file), "%s/features_original.txt", result_folder); std::ofstream feature_writer(feature_file, std::ofstream::out | std::ofstream::app); feature_writer << cur_features.bottle.loc[0] << " " << cur_features.bottle.loc[1] << " " << cur_features.bottle.loc[2] << " " << cur_features.bottle.ori[0] << " " << cur_features.bottle.ori[1] << " " << cur_features.bottle.ori[2] << " " << cur_features.bottle.color[0] << " " << cur_features.bottle.color[1] << " " << cur_features.bottle.color[2] << " " << cur_features.bottle.size[0] << " " << cur_features.bottle.size[1] << " " << cur_features.gripper_1.loc[0] << " " << cur_features.gripper_1.loc[1] << " " << cur_features.gripper_1.loc[2] << " " << cur_features.gripper_2.loc[0] << " " << cur_features.gripper_2.loc[1] << " " << cur_features.gripper_2.loc[2] << std::endl; feature_writer.close(); std::cout << "features saved at " << feature_file << std::endl; } char screenshot[200]; // make sure it's big enough std::snprintf(screenshot, sizeof(screenshot), "%s/video/sec_%d.png", result_folder, (int)video_id); std::cout << screenshot << std::endl; result_viewer->saveScreenshot(screenshot); idx=idx+step; video_id=video_id+1; } mainGraph.displayMainGraph(); mainGraph.recordMainGraph(mainGraph_file); while (!result_viewer->wasStopped ()) { result_viewer->spinOnce (100); boost::this_thread::sleep (boost::posix_time::microseconds (100000)); } }
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input) { // std::cout << "\n\n----------------Received point cloud!-----------------\n"; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsampled (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_planar (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_objects (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_red (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_green (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_blue (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_yellow (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_concat_clusters (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_concat_hulls (new pcl::PointCloud<pcl::PointXYZRGB>); sensor_msgs::PointCloud2 downsampled2, planar2, objects2, filtered2, red2, green2, blue2, yellow2, concat_clusters2, concat_hulls2; std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> color_clouds, cluster_clouds, hull_clouds; std::vector<pcl::PointXYZRGB> labels; // fromROSMsg(*input, *cloud); // pub_input.publish(*input); // Downsample the input PointCloud pcl::VoxelGrid<sensor_msgs::PointCloud2> sor; sor.setInputCloud (input); // sor.setLeafSize (0.01, 0.01, 0.01); //play around with leafsize (more samples, better resolution?) sor.setLeafSize (0.001, 0.001, 0.001); sor.filter (downsampled2); fromROSMsg(downsampled2,*downsampled); pub_downsampled.publish (downsampled2); // Segment the largest planar component from the downsampled cloud pcl::SACSegmentation<pcl::PointXYZRGB> seg; pcl::ModelCoefficients::Ptr coeffs (new pcl::ModelCoefficients ()); pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (100); seg.setDistanceThreshold (0.0085); seg.setInputCloud (downsampled); seg.segment (*inliers, *coeffs); // Extract the planar inliers from the input cloud pcl::ExtractIndices<pcl::PointXYZRGB> extract; extract.setInputCloud (downsampled); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloud_planar); // toROSMsg(*cloud_planar,planar2); // pub_planar.publish (planar2); // Remove the planar inliers, extract the rest extract.setNegative (true); extract.filter (*cloud_objects); // toROSMsg(*cloud_objects,objects2); // pub_objects.publish (objects2); // PassThrough Filter pcl::PassThrough<pcl::PointXYZRGB> pass; pass.setInputCloud (cloud_objects); pass.setFilterFieldName ("z"); //all duplos in pcd1 pass.setFilterLimits (0.8, 1.0); pass.filter (*cloud_filtered); toROSMsg(*cloud_filtered,filtered2); pub_filtered.publish (filtered2); //don't passthrough filter, does color filter work too? (cloud_red has many points in top right off the table) // Segment filtered PointCloud by color (red, green, blue, yellow) for (size_t i = 0 ; i < cloud_filtered->points.size () ; i++) { if ( (int(cloud_filtered->points[i].r) > 2*int(cloud_filtered->points[i].g)) && (cloud_filtered->points[i].r > cloud_filtered->points[i].b) ) cloud_red->points.push_back(cloud_filtered->points[i]); if ( (cloud_filtered->points[i].g > cloud_filtered->points[i].r) && (cloud_filtered->points[i].g > cloud_filtered->points[i].b) ) cloud_green->points.push_back(cloud_filtered->points[i]); if ( (cloud_filtered->points[i].b > cloud_filtered->points[i].r) && (cloud_filtered->points[i].b > cloud_filtered->points[i].g) ) cloud_blue->points.push_back(cloud_filtered->points[i]); if ( (cloud_filtered->points[i].r > cloud_filtered->points[i].g) && (int(cloud_filtered->points[i].g) - int(cloud_filtered->points[i].b) > 30) ) cloud_yellow->points.push_back(cloud_filtered->points[i]); } cloud_red->header.frame_id = "base_link"; cloud_red->width = cloud_red->points.size (); cloud_red->height = 1; color_clouds.push_back(cloud_red); toROSMsg(*cloud_red,red2); pub_red.publish (red2); cloud_green->header.frame_id = "base_link"; cloud_green->width = cloud_green->points.size (); cloud_green->height = 1; color_clouds.push_back(cloud_green); toROSMsg(*cloud_green,green2); pub_green.publish (green2); cloud_blue->header.frame_id = "base_link"; cloud_blue->width = cloud_blue->points.size (); cloud_blue->height = 1; color_clouds.push_back(cloud_blue); toROSMsg(*cloud_blue,blue2); pub_blue.publish (blue2); cloud_yellow->header.frame_id = "base_link"; cloud_yellow->width = cloud_yellow->points.size (); cloud_yellow->height = 1; color_clouds.push_back(cloud_yellow); toROSMsg(*cloud_yellow,yellow2); pub_yellow.publish (yellow2); // Extract Euclidian clusters from color-segmented PointClouds int j(0), num_red (0), num_green(0), num_blue(0), num_yellow(0); for (size_t cit = 0 ; cit < color_clouds.size() ; cit++) { pcl::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZRGB>); tree->setInputCloud (color_clouds[cit]); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec; ec.setClusterTolerance (0.0075); // 0.01 // ec.setMinClusterSize (12); // ec.setMaxClusterSize (75); ec.setMinClusterSize (100); ec.setMaxClusterSize (4000); ec.setSearchMethod (tree); ec.setInputCloud (color_clouds[cit]); ec.extract (cluster_indices); for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>); for (std::vector<int>::const_iterator pit = it->indices.begin () ; pit != it->indices.end () ; pit++) cloud_cluster->points.push_back (color_clouds[cit]->points[*pit]); cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; cloud_cluster->header.frame_id = "base_link"; cluster_clouds.push_back(cloud_cluster); labels.push_back(cloud_cluster->points[0]); if (cit == 0) num_red++; if (cit == 1) num_green++; if (cit == 2) num_blue++; if (cit == 3) num_yellow++; // Create ConvexHull for cluster (keep points on perimeter of cluster) pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::ConvexHull<pcl::PointXYZRGB> chull; chull.setInputCloud (cloud_cluster); chull.reconstruct (*cloud_hull); cloud_hull->width = cloud_hull->points.size (); cloud_hull->height = 1; cloud_hull->header.frame_id = "base_link"; hull_clouds.push_back(cloud_hull); j++; } } std::cout << "Number of RED clusters: " << num_red << std::endl; std::cout << "Number of GREEN clusters: " << num_green << std::endl; std::cout << "Number of BLUE clusters: " << num_blue << std::endl; std::cout << "Number of YELLOW clusters: " << num_yellow << std::endl; std::cout << "TOTAL number of clusters: " << j << std::endl; // Concatenate PointCloud clusters and convex hulls for (size_t k = 0 ; k < cluster_clouds.size() ; k++) { for (size_t l = 0 ; l < cluster_clouds[k]->size() ; l++) { cloud_concat_clusters->points.push_back(cluster_clouds[k]->points[l]); cloud_concat_hulls->points.push_back(hull_clouds[k]->points[l]); } } cloud_concat_clusters->header.frame_id = "base_link"; cloud_concat_clusters->width = cloud_concat_clusters->points.size (); cloud_concat_clusters->height = 1; toROSMsg(*cloud_concat_clusters,concat_clusters2); pub_concat_clusters.publish (concat_clusters2); cloud_concat_hulls->header.frame_id = "base_link"; cloud_concat_hulls->width = cloud_concat_hulls->points.size (); cloud_concat_hulls->height = 1; toROSMsg(*cloud_concat_hulls,concat_hulls2); pub_concat_hulls.publish (concat_hulls2); // Estimate the volume of each cluster double height, width; std::vector <double> heights, widths; std::vector <int> height_ids, width_ids; for (size_t k = 0 ; k < cluster_clouds.size() ; k++) { // Calculate cluster height double tallest(0), shortest(1000), widest(0) ; for (size_t l = 0 ; l < cluster_clouds[k]->size() ; l++) { double point_to_plane_dist; point_to_plane_dist = coeffs->values[0] * cluster_clouds[k]->points[l].x + coeffs->values[1] * cluster_clouds[k]->points[l].y + coeffs->values[2] * cluster_clouds[k]->points[l].z + coeffs->values[3] / sqrt( pow(coeffs->values[0], 2) + pow(coeffs->values[1], 2)+ pow(coeffs->values[2], 2) ); if (point_to_plane_dist < 0) point_to_plane_dist = 0; if (point_to_plane_dist > tallest) tallest = point_to_plane_dist; if (point_to_plane_dist < shortest) shortest = point_to_plane_dist; } // Calculate cluster width for (size_t m = 0 ; m < hull_clouds[k]->size() ; m++) { double parallel_vector_dist; for (size_t n = m ; n < hull_clouds[k]->size()-1 ; n++) { parallel_vector_dist = sqrt( pow(hull_clouds[k]->points[m].x - hull_clouds[k]->points[n+1].x,2) + pow(hull_clouds[k]->points[m].y - hull_clouds[k]->points[n+1].y,2) + pow(hull_clouds[k]->points[m].z - hull_clouds[k]->points[n+1].z,2) ); if (parallel_vector_dist > widest) widest = parallel_vector_dist; } } // Classify block heights (error +/- .005m) height = (shortest < .01) ? tallest : tallest - shortest; //check for stacked blocks heights.push_back(height); if (height > .020 && height < .032) height_ids.push_back(0); //0: standing flat else if (height > .036 && height < .043) height_ids.push_back(1); //1: standing side else if (height > .064) height_ids.push_back(2); //2: standing long else height_ids.push_back(-1); //height not classified // Classify block widths (error +/- .005m) width = widest; widths.push_back(widest); if (width > .032-.005 && width < .0515+.005) width_ids.push_back(1); //1: short else if (width > .065-.005 && width < .0763+.005) width_ids.push_back(2); //2: medium else if (width > .1275-.005 && width < .1554+.005) width_ids.push_back(4); //4: long else width_ids.push_back(-1); //width not classified } // Classify block size using width information std::vector<int> block_ids, idx_1x1, idx_1x2, idx_1x4, idx_unclassified; int num_1x1(0), num_1x2(0), num_1x4(0), num_unclassified(0); for (size_t p = 0 ; p < width_ids.size() ; p++) { if (width_ids[p] == 1) { block_ids.push_back(1); //block is 1x1 idx_1x1.push_back(p); num_1x1++; } else if (width_ids[p] == 2) { block_ids.push_back(2); //block is 1x2 idx_1x2.push_back(p); num_1x2++; } else if (width_ids[p] == 4) { block_ids.push_back(4); //block is 1x4 idx_1x4.push_back(p); num_1x4++; } else { block_ids.push_back(-1); //block not classified idx_unclassified.push_back(p); num_unclassified++; } } // Determine Duplos of the same size std::cout << "\nThere are " << num_1x1 << " blocks of size 1x1 "; if (num_1x1>0) std::cout << "(cluster index: "; for (size_t q = 0 ; q < idx_1x1.size() ; q++) std::cout << idx_1x1[q] << ", "; if (num_1x1>0) std::cout << ")"; std::cout << "\nThere are " << num_1x2 << " blocks of size 1x2 "; if (num_1x2>0) std::cout << "(cluster index: "; for (size_t q = 0 ; q < idx_1x2.size() ; q++) std::cout << idx_1x2[q] << ", "; if (num_1x2>0) std::cout << ")"; std::cout << "\nThere are " << num_1x4 << " blocks of size 1x4 "; if (num_1x4>0) std::cout << "(cluster index: "; for (size_t q = 0 ; q < idx_1x4.size() ; q++) std::cout << idx_1x4[q] << ", "; if (num_1x4>0) std::cout << ")"; std::cout << "\nThere are " << num_unclassified << " unclassified blocks "; if (num_unclassified>0) std::cout << "(cluster index: "; for (size_t q = 0 ; q < idx_unclassified.size() ; q++) std::cout << idx_unclassified[q] << ", "; if (num_unclassified>0) std::cout << ")"; std::cout << "\n\n\n"; return; }
// input: cloudInput // input: pointCloudNormals // output: cloudOutput // output: pointCloudNormalsFiltered void extractHandles(PointCloud::Ptr& cloudInput, PointCloud::Ptr& cloudOutput, PointCloudNormal::Ptr& pointCloudNormals, std::vector<int>& handles) { // PCL objects //pcl::PassThrough<Point> vgrid_; // Filtering + downsampling object pcl::VoxelGrid<Point> vgrid_; // Filtering + downsampling object pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_; // Planar segmentation object pcl::SACSegmentation<Point> seg_line_; // Planar segmentation object pcl::ProjectInliers<Point> proj_; // Inlier projection object pcl::ExtractIndices<Point> extract_; // Extract (too) big tables pcl::ConvexHull<Point> chull_; pcl::ExtractPolygonalPrismData<Point> prism_; pcl::PointCloud<Point> cloud_objects_; pcl::EuclideanClusterExtraction<Point> cluster_, handle_cluster_; pcl::PCDWriter writer; double sac_distance_, normal_distance_weight_; double eps_angle_, seg_prob_; int max_iter_; sac_distance_ = 0.05; //0.02 normal_distance_weight_ = 0.05; max_iter_ = 500; eps_angle_ = 30.0; //20.0 seg_prob_ = 0.99; btVector3 axis(0.0, 0.0, 1.0); seg_.setModelType(pcl::SACMODEL_NORMAL_PARALLEL_PLANE); seg_.setMethodType(pcl::SAC_RANSAC); seg_.setDistanceThreshold(sac_distance_); seg_.setNormalDistanceWeight(normal_distance_weight_); seg_.setOptimizeCoefficients(true); seg_.setAxis(Eigen::Vector3f(fabs(axis.getX()), fabs(axis.getY()), fabs(axis.getZ()))); seg_.setEpsAngle(pcl::deg2rad(eps_angle_)); seg_.setMaxIterations(max_iter_); seg_.setProbability(seg_prob_); cluster_.setClusterTolerance(0.03); cluster_.setMinClusterSize(200); KdTreePtr clusters_tree_(new KdTree); clusters_tree_->setEpsilon(1); cluster_.setSearchMethod(clusters_tree_); seg_line_.setModelType(pcl::SACMODEL_LINE); seg_line_.setMethodType(pcl::SAC_RANSAC); seg_line_.setDistanceThreshold(0.05); seg_line_.setOptimizeCoefficients(true); seg_line_.setMaxIterations(max_iter_); seg_line_.setProbability(seg_prob_); //filter cloud double leafSize = 0.001; pcl::VoxelGrid<Point> sor; sor.setInputCloud (cloudInput); sor.setLeafSize (leafSize, leafSize, leafSize); sor.filter (*cloudOutput); //sor.setInputCloud (pointCloudNormals); //sor.filter (*pointCloudNormalsFiltered); //std::vector<int> tempIndices; //pcl::removeNaNFromPointCloud(*cloudInput, *cloudOutput, tempIndices); //pcl::removeNaNFromPointCloud(*pointCloudNormals, *pointCloudNormalsFiltered, tempIndices); // Segment planes pcl::OrganizedMultiPlaneSegmentation<Point, PointNormal, pcl::Label> mps; ROS_INFO("Segmenting planes"); mps.setMinInliers (20000); mps.setMaximumCurvature(0.02); mps.setInputNormals (pointCloudNormals); mps.setInputCloud (cloudInput); std::vector<pcl::PlanarRegion<Point> > regions; std::vector<pcl::PointIndices> regionPoints; std::vector< pcl::ModelCoefficients > planes_coeff; mps.segment(planes_coeff, regionPoints); ROS_INFO_STREAM("Number of regions:" << regionPoints.size()); if ((int) regionPoints.size() < 1) { ROS_ERROR("no planes found"); return; } std::stringstream filename; for (size_t plane = 0; plane < regionPoints.size (); plane++) { filename.str(""); filename << "plane" << plane << ".pcd"; writer.write(filename.str(), *cloudInput, regionPoints[plane].indices, true); ROS_INFO("Plane model: [%f, %f, %f, %f] with %d inliers.", planes_coeff[plane].values[0], planes_coeff[plane].values[1], planes_coeff[plane].values[2], planes_coeff[plane].values[3], (int)regionPoints[plane].indices.size ()); //Project Points into the Perfect plane PointCloud::Ptr cloud_projected(new PointCloud()); pcl::PointIndicesPtr cloudPlaneIndicesPtr(new pcl::PointIndices(regionPoints[plane])); pcl::ModelCoefficientsPtr coeff(new pcl::ModelCoefficients(planes_coeff[plane])); proj_.setInputCloud(cloudInput); proj_.setIndices(cloudPlaneIndicesPtr); proj_.setModelCoefficients(coeff); proj_.setModelType(pcl::SACMODEL_PARALLEL_PLANE); proj_.filter(*cloud_projected); PointCloud::Ptr cloud_hull(new PointCloud()); // Create a Convex Hull representation of the projected inliers chull_.setInputCloud(cloud_projected); chull_.reconstruct(*cloud_hull); ROS_INFO("Convex hull has: %d data points.", (int)cloud_hull->points.size ()); if ((int) cloud_hull->points.size() == 0) { ROS_WARN("Convex hull has: %d data points. Returning.", (int)cloud_hull->points.size ()); return; } // Extract the handle clusters using a polygonal prism pcl::PointIndices::Ptr handlesIndicesPtr(new pcl::PointIndices()); prism_.setHeightLimits(0.05, 0.1); prism_.setInputCloud(cloudInput); prism_.setInputPlanarHull(cloud_hull); prism_.segment(*handlesIndicesPtr); ROS_INFO("Number of handle candidates: %d.", (int)handlesIndicesPtr->indices.size ()); if((int)handlesIndicesPtr->indices.size () < 1100) continue; /*######### handle clustering code handle_clusters.clear(); handle_cluster_.setClusterTolerance(0.03); handle_cluster_.setMinClusterSize(200); handle_cluster_.setSearchMethod(clusters_tree_); handle_cluster_.setInputCloud(handles); handle_cluster_.setIndices(handlesIndicesPtr); handle_cluster_.extract(handle_clusters); for(size_t j = 0; j < handle_clusters.size(); j++) { filename.str(""); filename << "handle" << (int)i << "-" << (int)j << ".pcd"; writer.write(filename.str(), *handles, handle_clusters[j].indices, true); }*/ pcl::StatisticalOutlierRemoval<Point> sor; PointCloud::Ptr cloud_filtered (new pcl::PointCloud<Point>); sor.setInputCloud (cloudInput); sor.setIndices(handlesIndicesPtr); sor.setMeanK (50); sor.setStddevMulThresh (1.0); sor.filter (*cloud_filtered); PointCloudNormal::Ptr cloud_filtered_hack (new PointCloudNormal); pcl::copyPointCloud(*cloud_filtered, *cloud_filtered_hack); // Our handle is in cloud_filtered. We want indices of a cloud (also filtered for NaNs) pcl::KdTreeFLANN<PointNormal> kdtreeNN; std::vector<int> pointIdxNKNSearch(1); std::vector<float> pointNKNSquaredDistance(1); kdtreeNN.setInputCloud(pointCloudNormals); for(size_t j = 0; j < cloud_filtered_hack->points.size(); j++) { kdtreeNN.nearestKSearch(cloud_filtered_hack->points[j], 1, pointIdxNKNSearch, pointNKNSquaredDistance); handles.push_back(pointIdxNKNSearch[0]); } } }
/** * 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; } }