void topicCallback_input(const sensor_msgs::PointCloud2::ConstPtr& msg) { /* protected region user implementation of subscribe callback for input on begin */ pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2 ()); pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ()); // Transformation into PCL type PointCloud2 pcl_conversions::toPCL((*msg), *(pcl_pc)); // Transformation into PCL type PointCloud<pcl::PointXYZRGB> pcl::fromPCLPointCloud2(*(pcl_pc), *(pcl_cloud)); //////////////////////// // PassThrough filter // //////////////////////// pcl::ConditionOr<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionOr<pcl::PointXYZ> ()); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, robot_pose_x+localconfig.inhib_size))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, robot_pose_x-localconfig.inhib_size))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, robot_pose_y+localconfig.inhib_size))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, robot_pose_y-localconfig.inhib_size))); // build the filter pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond); condrem.setInputCloud (pcl_cloud); condrem.setKeepOrganized(true); // apply filter condrem.filter (*pcl_cloud); // Transformation into ROS type pcl::toPCLPointCloud2(*(pcl_cloud), *(cloud_out)); pcl_conversions::moveFromPCL(*(cloud_out), output_pcloud2); output_ready = true; /* protected region user implementation of subscribe callback for input end */ }
void My_Filter::pclCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud){ pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2 ()); pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr final2 (new pcl::PointCloud<pcl::PointXYZ>); pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ()); sensor_msgs::PointCloud2 pc2; std::vector<int> inliers; // Transformation into PCL type PointCloud2 pcl_conversions::toPCL((*cloud), *(pcl_pc)); // Transformation into PCL type PointCloud<pcl::PointXYZRGB> pcl::fromPCLPointCloud2(*(pcl_pc), *(pcl_cloud)); std::list<geometry_msgs::PoseStamped> tmp_list; int robot_counter = 0; for (std::list<geometry_msgs::PoseStamped>::iterator it = robots.begin() ; it != robots.end(); ++it) { if(it->pose.orientation.x > 0.001) { // Robot currently not seen //////////////////////// // PassThrough filter // //////////////////////// pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (pcl_cloud); pass.setFilterFieldName ("x"); pass.setFilterLimits (it->pose.position.x-DETECTION_DISTANCE-it->pose.orientation.x*0.002, it->pose.position.x+DETECTION_DISTANCE+it->pose.orientation.x*0.002); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pass.setInputCloud (final2); pass.setFilterFieldName ("y"); pass.setFilterLimits (it->pose.position.y-DETECTION_DISTANCE-it->pose.orientation.x*0.002, it->pose.position.y+DETECTION_DISTANCE+it->pose.orientation.x*0.002); //pass.setFilterLimitsNegative (true); pass.filter (*final2); } else { // Robot seen //////////////////////// // PassThrough filter // //////////////////////// pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (pcl_cloud); pass.setFilterFieldName ("x"); pass.setFilterLimits (it->pose.position.x-DETECTION_DISTANCE, it->pose.position.x+DETECTION_DISTANCE); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pass.setInputCloud (final2); pass.setFilterFieldName ("y"); pass.setFilterLimits (it->pose.position.y-DETECTION_DISTANCE, it->pose.position.y+DETECTION_DISTANCE); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pcl::ConditionOr<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionOr<pcl::PointXYZ> ()); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, it->pose.position.x+DETECTION_DISTANCE))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, it->pose.position.x-DETECTION_DISTANCE))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, it->pose.position.y+DETECTION_DISTANCE))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, it->pose.position.y-DETECTION_DISTANCE))); // build the filter pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond); condrem.setInputCloud (pcl_cloud); condrem.setKeepOrganized(true); // apply filter condrem.filter (*pcl_cloud); } if(final2->size() > 2) { // Something in the box double meanX = 0.0; double meanY = 0.0; for(int i = 0; i < final2->size(); i++) { meanX += final2->at(i).x; meanY += final2->at(i).y; } meanX = meanX / final2->size(); meanY = meanY / final2->size(); //robots.at(robot_counter).pose.position.x = meanX; //robots.at(robot_counter).pose.position.y = meanY; it->pose.position.x = meanX; it->pose.position.y = meanY; char numstr[50]; sprintf(numstr, "/robot_%s_link", it->header.frame_id.c_str()); t = tf::StampedTransform(tf::Transform(tf::createQuaternionFromYaw(0.0), tf::Vector3(meanX, meanY, 0.0)), ros::Time::now(), "/world", numstr); t.stamp_ = ros::Time::now(); broadcaster.sendTransform(t); it->pose.orientation.x = 0.0; tmp_list.push_front(*it); //std::cout << "size : " << final2->size() << std::endl; } else { // Nothing found in the box it->pose.orientation.x += 1.0; if(it->pose.orientation.x > 100.0) { it->pose.orientation.x = 100.0; } //std::cout << "coef : " << it->pose.orientation.x << std::endl; geometry_msgs::PoseStamped tmp = *it; //robots.push_back(*it); //robots.erase(it); //robots.push_back(tmp); tmp_list.push_back(*it); } robot_counter++; } robots = tmp_list; ///////////////////////////////// // Statistical Outlier Removal // ///////////////////////////////// /* pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; sor.setInputCloud (pcl_cloud); sor.setMeanK (200); sor.setStddevMulThresh (1.0); sor.filter (*final); */ ////////////////////////////////// // Euclidian Cluster Extraction // ////////////////////////////////// // Creating the KdTree object for the search method of the extraction /* pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (pcl_cloud); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.05); // 2cm ec.setMinClusterSize (5); ec.setMaxClusterSize (50); ec.setSearchMethod (tree); ec.setInputCloud (pcl_cloud); ec.extract (cluster_indices); std::cout << cluster_indices.size() << std::endl; double x = 0.0; double y = 0.0; for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) { x = 0.0; y = 0.0; for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++) { x +=pcl_cloud->points[*pit].x; y +=pcl_cloud->points[*pit].y; } x = x / it->indices.size(); y = y / it->indices.size(); std::cout << "x : " << x << " y : " << y << " size : " << it->indices.size() << std::endl; } */ // Transformation into ROS type pcl::toPCLPointCloud2(*(pcl_cloud), *(cloud_out)); pcl_conversions::moveFromPCL(*(cloud_out), pc2); point_cloud_publisher_.publish(pc2); }
void topicCallback_input_cloud(const sensor_msgs::PointCloud2::ConstPtr& msg) { /* protected region user implementation of subscribe callback for input_cloud on begin */ pcl::PCLPointCloud2::Ptr pcl_pc(new pcl::PCLPointCloud2 ()); pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr final2 (new pcl::PointCloud<pcl::PointXYZ>); pcl::PCLPointCloud2::Ptr cloud_out (new pcl::PCLPointCloud2 ()); sensor_msgs::PointCloud2 pc2; std::vector<int> inliers; // Transformation into PCL type PointCloud2 pcl_conversions::toPCL((*msg), *(pcl_pc)); // Transformation into PCL type PointCloud<pcl::PointXYZRGB> pcl::fromPCLPointCloud2(*(pcl_pc), *(pcl_cloud)); std::list<geometry_msgs::PoseStamped> tmp_list; int robot_counter = 0; for (std::list<geometry_msgs::PoseStamped>::iterator it = robots.begin() ; it != robots.end(); ++it) { if(it->pose.orientation.x > 0.001) { // Robot currently not seen //////////////////////// // PassThrough filter // //////////////////////// pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (pcl_cloud); pass.setFilterFieldName ("x"); pass.setFilterLimits (it->pose.position.x-localconfig.detection_distance-it->pose.orientation.x*0.002, it->pose.position.x+localconfig.detection_distance+it->pose.orientation.x*0.002); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pass.setInputCloud (final2); pass.setFilterFieldName ("y"); pass.setFilterLimits (it->pose.position.y-localconfig.detection_distance-it->pose.orientation.x*0.002, it->pose.position.y+localconfig.detection_distance+it->pose.orientation.x*0.002); //pass.setFilterLimitsNegative (true); pass.filter (*final2); } else { // Robot seen //////////////////////// // PassThrough filter // //////////////////////// pcl::PassThrough<pcl::PointXYZ> pass; pass.setInputCloud (pcl_cloud); pass.setFilterFieldName ("x"); pass.setFilterLimits (it->pose.position.x-localconfig.detection_distance, it->pose.position.x+localconfig.detection_distance); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pass.setInputCloud (final2); pass.setFilterFieldName ("y"); pass.setFilterLimits (it->pose.position.y-localconfig.detection_distance, it->pose.position.y+localconfig.detection_distance); //pass.setFilterLimitsNegative (true); pass.filter (*final2); pcl::ConditionOr<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionOr<pcl::PointXYZ> ()); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::GT, it->pose.position.x+localconfig.detection_distance))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("x", pcl::ComparisonOps::LT, it->pose.position.x-localconfig.detection_distance))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::GT, it->pose.position.y+localconfig.detection_distance))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("y", pcl::ComparisonOps::LT, it->pose.position.y-localconfig.detection_distance))); // build the filter pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond); condrem.setInputCloud (pcl_cloud); condrem.setKeepOrganized(true); // apply filter condrem.filter (*pcl_cloud); } if(final2->size() > 2) { // Something in the box double meanX = 0.0; double meanY = 0.0; for(int i = 0; i < final2->size(); i++) { meanX += final2->at(i).x; meanY += final2->at(i).y; } meanX = meanX / final2->size(); meanY = meanY / final2->size(); //robots.at(robot_counter).pose.position.x = meanX; //robots.at(robot_counter).pose.position.y = meanY; it->pose.position.x = (meanX + it->pose.position.x) / 2.0; it->pose.position.y = (meanY + it->pose.position.y) / 2.0; char numstr[50]; sprintf(numstr, "/robot_%s_link", it->header.frame_id.c_str()); t = tf::StampedTransform(tf::Transform(tf::createQuaternionFromYaw(0.0), tf::Vector3(meanX, meanY, 0.0)), ros::Time::now(), "/world", numstr); t.stamp_ = ros::Time::now(); broadcaster.sendTransform(t); it->pose.orientation.x = 0.0; tmp_list.push_front(*it); if(it->header.frame_id == "0") { robot1_output_ready = true; } else { robot2_output_ready = true; } //std::cout << "size : " << final2->size() << std::endl; } else { // Nothing found in the box it->pose.orientation.x += 1.0; if(it->pose.orientation.x > 100.0) { it->pose.orientation.x = 100.0; } //std::cout << "coef : " << it->pose.orientation.x << std::endl; geometry_msgs::PoseStamped tmp = *it; //robots.push_back(*it); //robots.erase(it); //robots.push_back(tmp); tmp_list.push_back(*it); } robot_counter++; } robots = tmp_list; /* protected region user implementation of subscribe callback for input_cloud end */ }
void callback(const PCMsg::ConstPtr& kinect_pc_msg){ // Conversion from sensor_msgs::PointCloud2 to pcl::PointCloud pcl::fromROSMsg(*kinect_pc_msg, *kinects_pc_); // Remove NaNs vector<int> indices; pcl::removeNaNFromPointCloud<PointType>(*kinects_pc_, *kinects_pc_, indices); // Downsampling pc_downsampling<PointType>(kinects_pc_, 0.005,cluster_cloud_); // Get transform between pc frame and base_link if(first_frame_){ // get transform between the pc frame and the output frame get_transform(kinects_pc_->header.frame_id, "base_link", pc_transform_); first_frame_ = false; } // Transform the pc to base_link pcl::transformPointCloud(*cluster_cloud_, *cluster_cloud_, pc_transform_.translation, pc_transform_.rotation); kinects_pc_->header.frame_id = "base_link"; // Clip the pointcloud pcl::ConditionAnd<PointType>::Ptr height_cond (new pcl::ConditionAnd<PointType> ()); pcl::ConditionalRemoval<PointType> condrem (height_cond); height_cond->addComparison ( pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> ("z", pcl::ComparisonOps::GT, 0.15))); height_cond->addComparison ( pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> ("y", pcl::ComparisonOps::GT, -1.0))); height_cond->addComparison ( pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> ("y", pcl::ComparisonOps::LT, 1.0))); height_cond->addComparison ( pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> ("x", pcl::ComparisonOps::LT, 1.5))); height_cond->addComparison ( pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> ("x", pcl::ComparisonOps::GT, -0.5))); condrem.setInputCloud (cluster_cloud_); condrem.setKeepOrganized(true); condrem.filter (*cluster_cloud_); // Estimate normals norm_est_.setKSearch(20); norm_est_.setInputCloud (cluster_cloud_); norm_est_.compute (*normals_); // PCL Viewer if(first_frame_) viewer_.addPointCloudNormals<PointType, NormalType>(cluster_cloud_, normals_, 1, 0.05, "normals"); else{ viewer_.removePointCloud("normals"); viewer_.addPointCloudNormals<PointType, NormalType>(cluster_cloud_, normals_, 1, 0.05, "normals"); } viewer_.updatePointCloud(cluster_cloud_,"sample_cloud"); viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "normals"); viewer_.spinOnce(); bool use_hough_ (true), show_correspondences_(true), show_keypoints_(true); float model_ss_ (0.005f); float scene_ss_ (0.005f); float descr_rad_ (0.036f); //float rf_rad_ (0.1f); float rf_rad_(rf_rad); float cg_size_ (0.2f); float cg_thresh_ (3.0f); int knn = 1; pcl::PointCloud<int> model_keypoints_indices, scene_keypoints_indices; pcl::PointCloud<PointType>::Ptr model_keypoints, scene_keypoints; model_keypoints = boost::shared_ptr<pcl::PointCloud<PointType> >(new pcl::PointCloud<PointType> ); scene_keypoints = boost::shared_ptr<pcl::PointCloud<PointType> >(new pcl::PointCloud<PointType> ); pcl::UniformSampling<PointType> uniform_sampling; uniform_sampling.setInputCloud (robot_pc_); uniform_sampling.setRadiusSearch (model_ss_); uniform_sampling.compute(model_keypoints_indices); std::cout << "Model total points: " << robot_pc_->size () << "; Selected Keypoints: " << model_keypoints_indices.size () << std::endl; uniform_sampling.setInputCloud (cluster_cloud_); uniform_sampling.setRadiusSearch (scene_ss_);; uniform_sampling.compute(scene_keypoints_indices); std::cout << "Scene total points: " << cluster_cloud_->size () << "; Selected Keypoints: " << scene_keypoints_indices.size () << std::endl; // Use Keypoint indices to make a PointCloud model_keypoints->points.resize(model_keypoints_indices.points.size ()); for (size_t i=0; i<model_keypoints_indices.points.size (); ++i) model_keypoints->points[i].getVector3fMap () = robot_pc_->points[model_keypoints_indices.points[i]].getVector3fMap(); scene_keypoints->points.resize (scene_keypoints_indices.points.size ()); for (size_t i=0; i<scene_keypoints_indices.points.size (); ++i) scene_keypoints->points[i].getVector3fMap () = cluster_cloud_->points[scene_keypoints_indices.points[i]].getVector3fMap(); std::cout<<"keypoints converted"<<std::endl; // // Compute Descriptor for keypoints // pcl::PointCloud<DescriptorType>::Ptr model_descriptors (new pcl::PointCloud<DescriptorType> ()); pcl::PointCloud<DescriptorType>::Ptr scene_descriptors (new pcl::PointCloud<DescriptorType> ()); pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est; //descr_est.setKSearch(10); descr_est.setRadiusSearch (descr_rad_); descr_est.setInputCloud (model_keypoints); descr_est.setInputNormals (robot_normals_); descr_est.setSearchSurface (robot_pc_); descr_est.compute (*model_descriptors); std::cout<<"SHOTEstimationOMP done for model"<<std::endl; pcl::SHOTEstimationOMP<PointType, NormalType, DescriptorType> descr_est2; // descr_est2.setKSearch(10); descr_est2.setRadiusSearch (descr_rad_); descr_est2.setInputCloud (scene_keypoints); descr_est2.setInputNormals (normals_); descr_est2.setSearchSurface (cluster_cloud_); descr_est2.compute (*scene_descriptors); std::cout<<"SHOTEstimationOMP done for scene"<<std::endl; // // Find Model-Scene Correspondences with KdTree // pcl::CorrespondencesPtr model_scene_corrs (new pcl::Correspondences ()); pcl::KdTreeFLANN<DescriptorType> match_search; match_search.setInputCloud (model_descriptors); std::cout<<"KdTreeFLANN searched"<<std::endl; // For each scene keypoint descriptor, find nearest neighbor into the model keypoints descriptor cloud and add it to the correspondences vector. // for (size_t i = 0; i < scene_descriptors->size (); ++i) // { // std::vector<int> neigh_indices (1); // std::vector<float> neigh_sqr_dists (1); // if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs // { // continue; // } // int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), 1, neigh_indices, neigh_sqr_dists); // // if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design) // if(found_neighs == 1 && neigh_sqr_dists[0] < 0.25f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design) // { // pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]); // model_scene_corrs->push_back (corr); // } // } for (size_t i = 0; i < scene_descriptors->size (); ++i) { std::vector<int> neigh_indices (knn); std::vector<float> neigh_sqr_dists (knn); if (!pcl_isfinite (scene_descriptors->at (i).descriptor[0])) //skipping NaNs { continue; } int found_neighs = match_search.nearestKSearch (scene_descriptors->at (i), knn, neigh_indices, neigh_sqr_dists); for(int k = 0; k < found_neighs; k++) { if(found_neighs == 1 && neigh_sqr_dists[k] < 0.1f) // add match only if the squared descriptor distance is less than 0.25 (SHOT descriptor distances are between 0 and 1 by design) { pcl::Correspondence corr (neigh_indices[k], static_cast<int> (i), neigh_sqr_dists[k]); model_scene_corrs->push_back (corr); } } } std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl; // // Actual Clustering // std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations; std::vector<pcl::Correspondences> clustered_corrs; std::cout<<"Actual clustering done"<<std::endl; // Using Hough3D if (use_hough_) { // // Compute (Keypoints) Reference Frames only for Hough // pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ()); pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ()); pcl::BOARDLocalReferenceFrameEstimation<PointType, NormalType, RFType> rf_est; rf_est.setFindHoles (true); rf_est.setRadiusSearch (rf_rad_); rf_est.setInputCloud (model_keypoints); rf_est.setInputNormals (robot_normals_); rf_est.setSearchSurface (robot_pc_); rf_est.compute (*model_rf); rf_est.setInputCloud (scene_keypoints); rf_est.setInputNormals (normals_); rf_est.setSearchSurface (cluster_cloud_); rf_est.compute (*scene_rf); // Clustering pcl::Hough3DGrouping<PointType, PointType, RFType, RFType> clusterer; clusterer.setHoughBinSize (cg_size_); clusterer.setHoughThreshold (cg_thresh_); clusterer.setUseInterpolation (true); clusterer.setUseDistanceWeight (false); clusterer.setInputCloud (model_keypoints); clusterer.setInputRf (model_rf); clusterer.setSceneCloud (scene_keypoints); clusterer.setSceneRf (scene_rf); clusterer.setModelSceneCorrespondences (model_scene_corrs); //clusterer.cluster (clustered_corrs); clusterer.recognize (rototranslations, clustered_corrs); } else // Using GeometricConsistency { pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer; gc_clusterer.setGCSize (cg_size_); gc_clusterer.setGCThreshold (cg_thresh_); gc_clusterer.setInputCloud (model_keypoints); gc_clusterer.setSceneCloud (scene_keypoints); gc_clusterer.setModelSceneCorrespondences (model_scene_corrs); //gc_clusterer.cluster (clustered_corrs); gc_clusterer.recognize (rototranslations, clustered_corrs); } // // Output results // std::cout << "Model instances found: " << rototranslations.size () << std::endl; for (size_t i = 0; i < rototranslations.size (); ++i) { std::cout << "\n Instance " << i + 1 << ":" << std::endl; std::cout << " Correspondences belonging to this instance: " << clustered_corrs[i].size () << std::endl; // Print the rotation matrix and translation vector Eigen::Matrix3f rotation = rototranslations[i].block<3,3>(0, 0); Eigen::Vector3f translation = rototranslations[i].block<3,1>(0, 3); printf ("\n"); printf (" | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2)); printf (" R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2)); printf (" | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2)); printf ("\n"); printf (" t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2)); } // Visualization // White cloud: scene // Yellow cloud: off_scene_MODEL // Blue cloud: scene keypoints and off_scene_MODEL keypoints // Red cloud: rotated model // Green lines: correspondences between pcl::visualization::PCLVisualizer viewer ("Correspondence Grouping"); viewer.addPointCloud (cluster_cloud_, "scene_cloud"); pcl::PointCloud<PointType>::Ptr off_scene_model (new pcl::PointCloud<PointType> ()); pcl::PointCloud<PointType>::Ptr off_scene_model_keypoints (new pcl::PointCloud<PointType> ()); if (show_correspondences_ || show_keypoints_) { // We are translating the model so that it doesn't end in the middle of the scene representation pcl::transformPointCloud (*robot_pc_, *off_scene_model, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0)); pcl::transformPointCloud (*model_keypoints, *off_scene_model_keypoints, Eigen::Vector3f (-1,0,0), Eigen::Quaternionf (1, 0, 0, 0)); pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_color_handler (off_scene_model, 255, 255, 128); viewer.addPointCloud (off_scene_model, off_scene_model_color_handler, "off_scene_model"); } if (show_keypoints_) { pcl::visualization::PointCloudColorHandlerCustom<PointType> scene_keypoints_color_handler (scene_keypoints, 0, 0, 255); viewer.addPointCloud (scene_keypoints, scene_keypoints_color_handler, "scene_keypoints"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "scene_keypoints"); pcl::visualization::PointCloudColorHandlerCustom<PointType> off_scene_model_keypoints_color_handler (off_scene_model_keypoints, 0, 0, 255); viewer.addPointCloud (off_scene_model_keypoints, off_scene_model_keypoints_color_handler, "off_scene_model_keypoints"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "off_scene_model_keypoints"); } for (size_t i = 0; i < rototranslations.size (); ++i) { pcl::PointCloud<PointType>::Ptr rotated_model (new pcl::PointCloud<PointType> ()); pcl::transformPointCloud (*robot_pc_, *rotated_model, rototranslations[i]); std::stringstream ss_cloud; ss_cloud << "instance" << i; pcl::visualization::PointCloudColorHandlerCustom<PointType> rotated_model_color_handler (rotated_model, 255, 0, 0); viewer.addPointCloud (rotated_model, rotated_model_color_handler, ss_cloud.str ()); if (show_correspondences_) { for (size_t j = 0; j < clustered_corrs[i].size (); ++j) { std::stringstream ss_line; ss_line << "correspondence_line" << i << "_" << j; PointType& model_point = off_scene_model_keypoints->at (clustered_corrs[i][j].index_query); PointType& scene_point = scene_keypoints->at (clustered_corrs[i][j].index_match); // We are drawing a line for each pair of clustered correspondences found between the model and the scene viewer.addLine<PointType, PointType> (model_point, scene_point, 0, 255, 0, ss_line.str ()); } } } while (!viewer.wasStopped ()) { viewer.spinOnce (); } exit(0); }
bool filterIt(int argc, char **argv,pcl::PointCloud<PointType>::Ptr cloud, pcl::PointCloud<PointType>::Ptr cloud_filtered) { pcl::ScopeTime time("performance"); time.reset(); if (strcmp(argv[2], "-r") == 0) { if (argc != 5) { std::cout << "All the parameters are not correctly set for the radius removal filter" << std::endl; return 0; } pcl::RadiusOutlierRemoval<PointType> outrem; // build the filter outrem.setInputCloud(cloud); outrem.setRadiusSearch(atof(argv[3])); outrem.setMinNeighborsInRadius (atof(argv[4])); // apply filter outrem.filter (*cloud_filtered); } else if (strcmp(argv[2], "-c") == 0) { if (argc != 6) { std::cout << "All the parameters are not correctly set for the conditional filter" << std::endl; return 0; } // build the condition pcl::ConditionAnd<PointType>::Ptr range_cond (new pcl::ConditionAnd<PointType> ()); range_cond->addComparison (pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> (argv[5], pcl::ComparisonOps::GT, atof ( argv[3] )))); range_cond->addComparison (pcl::FieldComparison<PointType>::ConstPtr (new pcl::FieldComparison<PointType> (argv[5], pcl::ComparisonOps::LT, atof(argv[4]) ))); // build the filter pcl::ConditionalRemoval<PointType> condrem (range_cond); condrem.setInputCloud (cloud); condrem.setKeepOrganized(false); // apply filter condrem.filter (*cloud_filtered); } else if (strcmp(argv[2], "-s") == 0) { if (argc != 5) { std::cout << "All the parameters are not correctly set for the statistical filter" << std::endl; return 0; } // Create the filtering object pcl::StatisticalOutlierRemoval<PointType> sor; sor.setInputCloud (cloud); sor.setMeanK (atof(argv[3])); sor.setStddevMulThresh (atof(argv[4])); sor.filter (*cloud_filtered); } else if (strcmp(argv[2], "-v") == 0) { // Create the filtering object pcl::VoxelGrid<PointType> sor; sor.setDownsampleAllData(true); sor.setInputCloud (cloud); sor.setLeafSize (atof(argv[3]), atof(argv[3]), atof(argv[3])); sor.filter (*cloud_filtered); } else if (strcmp(argv[2], "-av") == 0) { // Create the filtering object pcl::ApproximateVoxelGrid<PointType> sor; sor.setDownsampleAllData(true); sor.setInputCloud (cloud); sor.setLeafSize (atof(argv[3]), atof(argv[3]), atof(argv[3])); sor.filter (*cloud_filtered); } else if (strcmp(argv[2], "-rg") == 0) { // Create the filtering object std::vector<pcl::IndicesPtr> clusteredIndices; pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>); pcl::RegionGrowing<PointType> sor; //sor.setDownsampleAllData(true); sor.setInputCloud (cloud); sor.setGrowingDistance(atof(argv[3])); //sor.setLeafSize (atof(argv[3]), atof(argv[3]), atof(argv[3])); if (atof(argv[4]) > 0){ sor.setEpsAngle(atof(argv[4])); pcl::NormalEstimation<PointType, pcl::Normal> ne; ne.setInputCloud (cloud); pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType> ()); ne.setSearchMethod (tree); ne.setRadiusSearch (atof(argv[3])); ne.compute (*cloud_normals); sor.setNormals(cloud_normals); cout << "normals estimed" << endl; } sor.cluster (&clusteredIndices); cout << "clusters? " <<clusteredIndices.size()<<endl; classification<PointType> classif(cloud, clusteredIndices); // pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType>); // tree->setInputCloud (cloud); //extractEuclideanClusters ( // const PointCloud<PointT> &cloud, const boost::shared_ptr<search::Search<PointT> > &tree, // float tolerance, std::vector<PointIndices> &clusters, // unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ()); // boost::shared_ptr<pcl::search::KdTree<PointType> > tree (new pcl::search::KdTree<PointType>); // tree->setInputCloud (cloud); // std::vector<pcl::PointIndices> clusters; // pcl::extractEuclideanClusters<PointType>(cloud, tree, 10, clusters, 100, 200); // pcl::EuclideanClusterExtraction<PointType> ec; // ec.setClusterTolerance ( atof(argv[3]) ); // 2cm // ec.setSearchMethod (tree); // ec.setInputCloud (cloud); // ec.extract (clusteredIndices); // cout << "clusters? " <<clusteredIndices.size()<<endl; pcl::PointCloud<PointType>::Ptr buff_cloud (new pcl::PointCloud<PointType>); for (int i=0; i<clusteredIndices.size(); i++) { //cout << "ok 2" << endl; if (clusteredIndices.operator[](i)->size() < 2300 || clusteredIndices.operator[](i)->size() > 2500) { // cout << "ok 3 " << i << endl; //cout << clusteredIndices[i]->size() << endl; pcl::copyPointCloud(*cloud, clusteredIndices[i].operator*(), *buff_cloud); cloud_filtered->operator+=(*buff_cloud); buff_cloud->clear(); } } if(system("mkdir clusters")) cout << "creating directory \"clusters\""<< endl; saveclusters(clusteredIndices, cloud, "clusters/"); std::stringstream command; command << "pcd_viewer "; for (int i =0 ; i< clusteredIndices.size(); i++) // if (clusteredIndices[i].indices.size() > 2300 && clusteredIndices[i].indices.size() < 2500) command << "clusters/" << i << ".pcd "; if(system (command.str().data())) cout << "visualizing clusters" << endl; FILE *fp; fp = fopen ( "command.sh", "wt" ) ; fprintf ( fp, " %s\n", command.str().data() ); fclose(fp); } else { std::cerr << "please specify the point cloud and the command line arg '-r' or '-c' or '-s' or '-v' or '-av' or 'rg' + param" << std::endl; exit(0); } time.getTime(); return 1; }
void pcl::kinfuLS::WorldModel<PointT>::getExistingData(const double previous_origin_x, const double previous_origin_y, const double previous_origin_z, const double offset_x, const double offset_y, const double offset_z, const double volume_x, const double volume_y, const double volume_z, pcl::PointCloud<PointT> &existing_slice) { double newOriginX = previous_origin_x + offset_x; double newOriginY = previous_origin_y + offset_y; double newOriginZ = previous_origin_z + offset_z; double newLimitX = newOriginX + volume_x; double newLimitY = newOriginY + volume_y; double newLimitZ = newOriginZ + volume_z; // filter points in the space of the new cube PointCloudPtr newCube (new pcl::PointCloud<PointT>); // condition ConditionAndPtr range_condAND (new pcl::ConditionAnd<PointT> ()); range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, newOriginX))); range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, newLimitX))); range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, newOriginY))); range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, newLimitY))); range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, newOriginZ))); range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, newLimitZ))); // build the filter pcl::ConditionalRemoval<PointT> condremAND (true); condremAND.setCondition (range_condAND); condremAND.setInputCloud (world_); condremAND.setKeepOrganized (false); // apply filter condremAND.filter (*newCube); // filter points that belong to the new slice ConditionOrPtr range_condOR (new pcl::ConditionOr<PointT> ()); if(offset_x >= 0) range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_origin_x + volume_x - 1.0 ))); else range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x ))); if(offset_y >= 0) range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_origin_y + volume_y - 1.0 ))); else range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y ))); if(offset_z >= 0) range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_origin_z + volume_z - 1.0 ))); else range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z ))); // build the filter pcl::ConditionalRemoval<PointT> condrem (true); condrem.setCondition (range_condOR); condrem.setInputCloud (newCube); condrem.setKeepOrganized (false); // apply filter condrem.filter (existing_slice); if(existing_slice.points.size () != 0) { //transform the slice in new cube coordinates Eigen::Affine3f transformation; transformation.translation ()[0] = newOriginX; transformation.translation ()[1] = newOriginY; transformation.translation ()[2] = newOriginZ; transformation.linear ().setIdentity (); transformPointCloud (existing_slice, existing_slice, transformation.inverse ()); } }
inline void normal_diff_filter( const pcl::PointCloud<PointT> &in, pcl::PointCloud<PointT> &out, double scale1 = 0.40, ///The smallest scale to use in the DoN filter. double scale2 = 0.60, ///The largest scale to use in the DoN filter. double threshold = 0.6 ///The minimum DoN magnitude to threshold by ) { boost::shared_ptr<pcl::PointCloud<PointT> > in_ptr(in.makeShared()); boost::shared_ptr<pcl::search::KdTree<PointT> > tree_n; tree_n.reset( new pcl::search::KdTree<PointT>(false) ); tree_n->setInputCloud(in_ptr); tree_n->setEpsilon(0.5); if (scale1 >= scale2) { printf("Error: Large scale must be > small scale!"); return; } // Compute normals using both small and large scales at each point pcl::NormalEstimationOMP<PointT, pcl::PointNormal> ne; ne.setInputCloud (in_ptr); ne.setSearchMethod (tree_n); /** * NOTE: setting viewpoint is very important, so that we can ensure * normals are all pointed in the same direction! */ ne.setViewPoint (std::numeric_limits<float>::max (), std::numeric_limits<float>::max (), std::numeric_limits<float>::max ()); // calculate normals with the small scale pcl::PointCloud<pcl::PointNormal>::Ptr normals_small_scale (new pcl::PointCloud<pcl::PointNormal>); ne.setRadiusSearch (scale1); ne.compute (*normals_small_scale); // calculate normals with the large scale pcl::PointCloud<pcl::PointNormal>::Ptr normals_large_scale (new pcl::PointCloud<pcl::PointNormal>); ne.setRadiusSearch (scale2); ne.compute (*normals_large_scale); // Create output cloud for DoN results pcl::PointCloud<pcl::PointNormal>::Ptr doncloud (new pcl::PointCloud<pcl::PointNormal>); pcl::copyPointCloud<PointT, pcl::PointNormal>(in, *doncloud); // Create DoN operator pcl::DifferenceOfNormalsEstimation<PointT, pcl::PointNormal, pcl::PointNormal> don; don.setInputCloud(in_ptr); don.setNormalScaleLarge(normals_large_scale); don.setNormalScaleSmall(normals_small_scale); if (!don.initCompute ()) { std::cerr << "Error: Could not intialize DoN feature operator" << std::endl; return; } // Compute DoN don.computeFeature (*doncloud); // Build the condition for filtering pcl::ConditionOr<pcl::PointNormal>::Ptr range_cond ( new pcl::ConditionOr<pcl::PointNormal> () ); range_cond->addComparison (pcl::FieldComparison<pcl::PointNormal>::ConstPtr ( new pcl::FieldComparison<pcl::PointNormal> ("curvature", pcl::ComparisonOps::GT, threshold)) ); // Build the filter pcl::ConditionalRemoval<pcl::PointNormal> condrem (range_cond, true); condrem.setInputCloud(doncloud); pcl::PointCloud<pcl::PointNormal>::Ptr doncloud_filtered (new pcl::PointCloud<pcl::PointNormal>); // Apply filter condrem.filter(*doncloud_filtered); boost::shared_ptr<pcl::PointIndices> indices(new pcl::PointIndices); condrem.getRemovedIndices(*indices); //std::cout << "Indices: " << indices->indices.size() << " Filtered: " << doncloud_filtered->size () << " data points." << std::endl; pcl::ExtractIndices<PointT> extract; extract.setInputCloud(in_ptr); extract.setIndices(indices); extract.setNegative (true); extract.filter(out); // Save filtered output //std::cout << "In: " << in.size() << " Filtered: " << out.size () << " data points." << std::endl; }
int main (int argc, char** argv) { if (argc != 2) { std::cerr << "please specify command line arg '-r' or '-c'" << std::endl; exit(0); } pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); // Fill in the cloud data cloud->width = 5; cloud->height = 1; cloud->points.resize (cloud->width * cloud->height); for (size_t i = 0; i < cloud->points.size (); ++i) { cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0); cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0); cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0); } if (strcmp(argv[1], "-r") == 0){ pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem; // build the filter outrem.setInputCloud(cloud); outrem.setRadiusSearch(0.8); outrem.setMinNeighborsInRadius (2); // apply filter outrem.filter (*cloud_filtered); } else if (strcmp(argv[1], "-c") == 0){ // build the condition pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond (new pcl::ConditionAnd<pcl::PointXYZ> ()); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::GT, 0.0))); range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ> ("z", pcl::ComparisonOps::LT, 0.8))); // build the filter pcl::ConditionalRemoval<pcl::PointXYZ> condrem (range_cond); condrem.setInputCloud (cloud); condrem.setKeepOrganized(true); // apply filter condrem.filter (*cloud_filtered); } else{ std::cerr << "please specify command line arg '-r' or '-c'" << std::endl; exit(0); } std::cerr << "Cloud before filtering: " << std::endl; for (size_t i = 0; i < cloud->points.size (); ++i) std::cerr << " " << cloud->points[i].x << " " << cloud->points[i].y << " " << cloud->points[i].z << std::endl; // display pointcloud after filtering std::cerr << "Cloud after filtering: " << std::endl; for (size_t i = 0; i < cloud_filtered->points.size (); ++i) std::cerr << " " << cloud_filtered->points[i].x << " " << cloud_filtered->points[i].y << " " << cloud_filtered->points[i].z << std::endl; return (0); }
void pcl::WorldModel<PointT>::getWorldAsCubes (const double size, std::vector<typename pcl::WorldModel<PointT>::PointCloudPtr> &cubes, std::vector<Eigen::Vector3f> &transforms, double overlap) { if(world_->points.size () == 0) { PCL_INFO("The world is empty, returning nothing\n"); return; } PCL_INFO ("Getting world as cubes. World contains %d points.\n", world_->points.size ()); // remove nans from world cloud world_->is_dense = false; std::vector<int> indices; pcl::removeNaNFromPointCloud ( *world_, *world_, indices); PCL_INFO ("World contains %d points after nan removal.\n", world_->points.size ()); // check cube size value double cubeSide = size; if (cubeSide <= 0.0f) { PCL_ERROR ("Size of the cube must be positive and non null (%f given). Setting it to 3.0 meters.\n", cubeSide); cubeSide = 512.0f; } std::cout << "cube size is set to " << cubeSide << std::endl; // check overlap value double step_increment = 1.0f - overlap; if (overlap < 0.0) { PCL_ERROR ("Overlap ratio must be positive or null (%f given). Setting it to 0.0 procent.\n", overlap); step_increment = 1.0f; } if (overlap > 1.0) { PCL_ERROR ("Overlap ratio must be less or equal to 1.0 (%f given). Setting it to 10 procent.\n", overlap); step_increment = 0.1f; } // get world's bounding values on XYZ PointT min, max; pcl::getMinMax3D(*world_, min, max); PCL_INFO ("Bounding box for the world: \n\t [%f - %f] \n\t [%f - %f] \n\t [%f - %f] \n", min.x, max.x, min.y, max.y, min.z, max.z); PointT origin = min; // clear returned vectors cubes.clear(); transforms.clear(); // iterate with box filter while (origin.x < max.x) { origin.y = min.y; while (origin.y < max.y) { origin.z = min.z; while (origin.z < max.z) { // extract cube here PCL_INFO ("Extracting cube at: [%f, %f, %f].\n", origin.x, origin.y, origin.z); // pointcloud for current cube. PointCloudPtr box (new pcl::PointCloud<PointT>); // set conditional filter ConditionAndPtr range_cond (new pcl::ConditionAnd<PointT> ()); range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, origin.x))); range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, origin.x + cubeSide))); range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, origin.y))); range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, origin.y + cubeSide))); range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, origin.z))); range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, origin.z + cubeSide))); // build the filter pcl::ConditionalRemoval<PointT> condrem (range_cond); condrem.setInputCloud (world_); condrem.setKeepOrganized(false); // apply filter condrem.filter (*box); // also push transform along with points. if(box->points.size() > 0) { Eigen::Vector3f transform; transform[0] = origin.x, transform[1] = origin.y, transform[2] = origin.z; transforms.push_back(transform); cubes.push_back(box); } else { PCL_INFO ("Extracted cube was empty, skiping this one.\n"); } origin.z += cubeSide * step_increment; } origin.y += cubeSide * step_increment; } origin.x += cubeSide * step_increment; } /* for(int c = 0 ; c < cubes.size() ; ++c) { std::stringstream name; name << "cloud" << c+1 << ".pcd"; pcl::io::savePCDFileASCII(name.str(), *(cubes[c])); }*/ std::cout << "returning " << cubes.size() << " cubes" << std::endl; }