void MakePlan::removeField(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud_out) { pcl::PointIndices::Ptr indices_x(new pcl::PointIndices); pcl::PointIndices::Ptr indices_xy(new pcl::PointIndices); pcl::PassThrough<pcl::PointXYZRGB> ptfilter; // Initializing with true will allow us to extract the removed indices ptfilter.setInputCloud (cloud_in); ptfilter.setFilterFieldName ("x"); ptfilter.setFilterLimits (-2.0, 2.0); ptfilter.filter (indices_x->indices); ptfilter.setIndices (indices_x); ptfilter.setFilterFieldName ("y"); ptfilter.setFilterLimits (-2.0, 2.0); ptfilter.filter (indices_xy->indices); ptfilter.setIndices (indices_xy); ptfilter.setFilterFieldName ("z"); ptfilter.setFilterLimits (0.5,2.0); ptfilter.filter (*cloud_out); }
void ColorizeRandomForest::extract(const sensor_msgs::PointCloud2 pc) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB> ()); std::vector<int> indices; pcl::fromROSMsg(pc, *cloud); cloud->is_dense = false; pcl::removeNaNFromPointCloud(*cloud, *cloud, indices); pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>); tree->setInputCloud (cloud); pcl::PassThrough<pcl::PointXYZRGB> pass; pass.setInputCloud (cloud); pass.setFilterFieldName (std::string("z")); pass.setFilterLimits (0.0, 1.5); pass.filter (*cloud); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec; ec.setClusterTolerance (0.025); ec.setMinClusterSize (200); ec.setMaxClusterSize (100000); ec.setSearchMethod (tree); ec.setInputCloud (cloud); ec.extract (cluster_indices); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloth_cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr noncloth_cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>); int cluster_num = 0; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { JSK_ROS_INFO("Calculate time %d / %ld", cluster_num , cluster_indices.size()); 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 (cloud->points[*pit]); cloud_cluster->is_dense = true; cluster_num ++ ; pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> ne; pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ()); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointXYZRGBNormal>); ne.setInputCloud (cloud_cluster); ne.setSearchMethod (tree); ne.setRadiusSearch (0.02); ne.compute (*cloud_normals); for (int cloud_index = 0; cloud_index < cloud_normals->points.size(); cloud_index++) { cloud_normals->points[cloud_index].x = cloud_cluster->points[cloud_index].x; cloud_normals->points[cloud_index].y = cloud_cluster->points[cloud_index].y; cloud_normals->points[cloud_index].z = cloud_cluster->points[cloud_index].z; } int result_counter=0, call_counter = 0; pcl::PointXYZRGBNormal max_pt,min_pt; pcl::getMinMax3D(*cloud_normals, min_pt, max_pt); for (int i = 0 ; i < 30; i++) { double lucky = 0, lucky2 = 0; std::string axis("x"), other_axis("y"); int rand_xy = rand()%2; if (rand_xy == 0) { lucky = min_pt.x - pass_offset_ + (max_pt.x - min_pt.x - pass_offset_*2) * 1.0 * rand() / RAND_MAX; lucky2 = min_pt.y + (max_pt.y - min_pt.y) * 1.0 * rand() / RAND_MAX; } else { lucky = min_pt.y - pass_offset_ + (max_pt.y - min_pt.y - pass_offset_*2) * 1.0 * rand() / RAND_MAX; lucky2 = min_pt.x + (max_pt.x - min_pt.x) * 1.0 * rand() / RAND_MAX; axis = std::string("y"); other_axis = std::string("x"); } pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_normals_pass (new pcl::PointCloud<pcl::PointXYZRGBNormal>); pcl::PassThrough<pcl::PointXYZRGBNormal> pass; pcl::IndicesPtr indices_x(new std::vector<int>()); pass.setInputCloud (cloud_normals); pass.setFilterFieldName (axis); float small = std::min(lucky, lucky + pass_offset_); float large = std::max(lucky, lucky + pass_offset_); pass.setFilterLimits (small, large); pass.filter (*cloud_normals_pass); pass.setInputCloud (cloud_normals_pass); pass.setFilterFieldName (other_axis); float small2 = std::min(lucky2, lucky2 + pass_offset2_); float large2 = std::max(lucky2, lucky2 + pass_offset2_); pass.setFilterLimits (small2, large2); pass.filter (*cloud_normals_pass); std::vector<int> tmp_indices; pcl::removeNaNFromPointCloud(*cloud_normals_pass, *cloud_normals_pass, tmp_indices); if(cloud_normals_pass->points.size() == 0) continue; pcl::FPFHEstimationOMP<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal, pcl::FPFHSignature33> fpfh; pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>); pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33> ()); fpfh.setNumberOfThreads(8); fpfh.setInputCloud (cloud_normals_pass); fpfh.setInputNormals (cloud_normals_pass); fpfh.setSearchMethod (tree); fpfh.setRadiusSearch (radius_search_); fpfh.compute (*fpfhs); if((int)fpfhs->points.size() == 0) continue; std::vector<double> result; int target_id, max_value; if ((int)fpfhs->points.size() - sum_num_ - 1 < 1) { target_id = 0; max_value = (int)fpfhs->points.size(); } else { target_id = rand() % ((int)fpfhs->points.size() - sum_num_ - 1); max_value = target_id + sum_num_; } bool has_nan = false; for(int index = 0; index < 33; index++) { float sum_hist_points = 0; for(int kndex = target_id; kndex < max_value; kndex++) { sum_hist_points+=fpfhs->points[kndex].histogram[index]; } result.push_back( sum_hist_points/sum_num_ ); } for(int x = 0; x < result.size(); x ++) { if(pcl_isnan(result[x])) has_nan = true; } if(has_nan) break; call_counter++; ros::ServiceClient client = pnh_->serviceClient<ml_classifiers::ClassifyData>("/predict"); ml_classifiers::ClassifyData srv; ml_classifiers::ClassDataPoint class_data_point; class_data_point.point = result; srv.request.data.push_back(class_data_point); if(client.call(srv)) if (atoi(srv.response.classifications[0].c_str()) == 0) result_counter += 1; JSK_ROS_INFO("response result : %s", srv.response.classifications[0].c_str()); } if(result_counter >= call_counter / 2) { JSK_ROS_INFO("Cloth %d / %d", result_counter, call_counter); } else { JSK_ROS_INFO("Not Cloth %d / %d", result_counter, call_counter); } for (int i = 0; i < cloud_cluster->points.size(); i++) { if(result_counter >= call_counter / 2) { cloth_cloud_cluster->points.push_back (cloud_cluster->points[i]); } else { noncloth_cloud_cluster->points.push_back (cloud_cluster->points[i]); } } } sensor_msgs::PointCloud2 cloth_pointcloud2; pcl::toROSMsg(*cloth_cloud_cluster, cloth_pointcloud2); cloth_pointcloud2.header = pc.header; cloth_pointcloud2.is_dense = false; pub_.publish(cloth_pointcloud2); sensor_msgs::PointCloud2 noncloth_pointcloud2; pcl::toROSMsg(*noncloth_cloud_cluster, noncloth_pointcloud2); noncloth_pointcloud2.header = pc.header; noncloth_pointcloud2.is_dense = false; pub2_.publish(noncloth_pointcloud2); }