void init(const OpenROVmessages::LaserMsg msg){ /********************* INITIALIZE CLOUDS *********************/ // Mandatory: Init he sizes of the point clouds temp_cloud->width = msg.n_rois; temp_cloud->height = 2; temp_cloud->resize(temp_cloud->width * temp_cloud->height); nr_points = temp_cloud->points.size(); /**************** INITIALIZE SEGMENTER ****************/ // Mandatory: use perpendicular plane to use setAxis() function seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE); // seg.setOptimizeCoefficients (true); // Set method and threshold seg.setMethodType (pcl::SAC_RANSAC); // seg.setDistanceThreshold (msg.ransac_threshold); seg.setDistanceThreshold (5); // Set the axis for which to search for perpendicular planes Eigen::Vector3f axis = Eigen::Vector3f(1.0,1.0,0.0); // Set a allowed deviation angle from vector axis seg.setEpsAngle((60*CV_PI)/180); seg.setAxis(axis); }
int main(int argc, char** argv) { ros::init(argc, argv, "downsample"); ros::NodeHandle n; downsample_pub = n.advertise<sensor_msgs::PointCloud2>("/downsample", 10); depth_sub = n.subscribe("/camera/depth/points", 10, depthPointsCallback); ros::Rate loop_rate(10); // Set variables for filters vox.setLeafSize (0.01, 0.01, 0.01); sor.setMeanK (10); sor.setStddevMulThresh (0.1); ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (20); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); //seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setDistanceThreshold (0.02); while(ros::ok()) { ros::spin(); } return 0; }
int find_planes(const OpenROVmessages::LaserMsg msg){ ROS_INFO("Entered find_planes"); // Fill cloud with data fill_cloud(msg); // Find planes in the cloud while(temp_cloud->points.size() > 0.1 * nr_points){ seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE); seg.setOptimizeCoefficients (true); // Set method and threshold seg.setMethodType (pcl::SAC_RANSAC); seg.setDistanceThreshold (msg.ransac_threshold); // Set the axis for which to search for perpendicular planes Eigen::Vector3f axis = Eigen::Vector3f(1.0,0.0,0.0); // Set a allowed deviation angle from vector axis seg.setEpsAngle((60*CV_PI)/180); seg.setAxis(axis); // Segment the planes and insert the coefficients and inliers in vectors seg.setInputCloud (temp_cloud); seg.segment (*inliers, *coefficients); // Check that we found a plane from the cloud if (inliers->indices.size () == 0) { std::cerr << "Could not estimate a planar model for the given dataset." << std::endl; break; } // push coeffients onto stack coef_vect.push_back(*coefficients); // Extract the inliers extract.setInputCloud (temp_cloud); extract.setIndices (inliers); // Extract found inliers from cloud extract.setNegative(true); extract.filter(*temp_cloud); } return 0; }
int main(int argc, char **argv){ if (argc < 2){ printf("Error: Debe especificar un archivo .pcd\n"); exit(1); } // Cargar nube de puntos PointCloud::Ptr cloud (new PointCloud); pcl::io::loadPCDFile(argv[1],*cloud); // Crear visualizador boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = simpleVis(cloud); // Segmentar segmentator.setOptimizeCoefficients(true); segmentator.setModelType(pcl::SACMODEL_PLANE); segmentator.setMethodType(pcl::SAC_RANSAC); segmentator.setDistanceThreshold(SEG_THRESHOLD); segmentator.setInputCloud(cloud); pcl::ModelCoefficients::Ptr coefs (new pcl::ModelCoefficients()); pcl::PointIndices::Ptr inliers (new pcl::PointIndices()); segmentator.segment(*inliers,*coefs); // Ver qué se obtuvo printf("Se obtienen %d puntos\n",(int)inliers->indices.size()); if (inliers->indices.size() == 0){ printf("Segmentación no sirvió de nada.\n"); exit(1); } viewer->addPlane(*coefs, "planito"); // Obtener centroide y dibujarlo Eigen::Vector4f centroid; compute3DCentroid(*cloud,centroid); viewer->addSphere(pcl::PointXYZ(centroid(0),centroid(1),centroid(2)), 0.2, "centroide"); // Dibujar un plano en el viewer para indicar más menos qué se obtuvo // http://docs.pointclouds.org/trunk/classpcl_1_1visualization_1_1_p_c_l_visualizer.html while (!viewer->wasStopped()){ viewer->spinOnce(100); boost::this_thread::sleep (boost::posix_time::microseconds (100000)); } }
void depthPointsCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) { // Instantiate various clouds pcl::PCLPointCloud2* cloud_intermediate = new pcl::PCLPointCloud2; pcl::PCLPointCloud2ConstPtr cloudPtr(cloud_intermediate); pcl::PointCloud<pcl::PointXYZ> cloud; // Convert to PCL data type pcl_conversions::toPCL(*cloud_msg, *cloud_intermediate); // Apply Voxel Filter on PCLPointCloud2 vox.setInputCloud (cloudPtr); vox.setInputCloud (cloudPtr); vox.filter (*cloud_intermediate); // Convert PCL::PointCloud2 to PCL::PointCloud<PointXYZ> pcl::fromPCLPointCloud2(*cloud_intermediate, cloud); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p = cloud.makeShared(); // Apply Passthrough Filter pass.setFilterFieldName ("z"); pass.setFilterLimits (0.3, 1); pass.setInputCloud (cloud_p); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_p); // Apply Passthrough Filter pass.setFilterFieldName ("x"); pass.setFilterLimits (-0.2, 0.2); pass.setInputCloud (cloud_p); pass.setFilterFieldName ("z"); pass.setFilterLimits (0.0, 3.0); //pass.setFilterLimitsNegative (true); pass.filter (*cloud_p); // Apply Statistical Noise Filter sor.setInputCloud (cloud_p); sor.filter (*cloud_p); // Planar segmentation: Remove large planes? Or extract floor? pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers (new pcl::PointIndices); int nr_points = (int) cloud_p->points.size (); Eigen::Vector3f lol (0, 1, 0); seg.setEpsAngle( 30.0f * (3.14f/180.0f) ); seg.setAxis(lol); //while(cloud_p->points.size () > 0.2 * nr_points) { sor.setInputCloud (cloud_p); sor.filter (*cloud_p); // 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_p); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { //break; } else { /*std::cout << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << "\n";*/ pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(cloud_p); extract.setIndices(inliers); extract.setNegative(true); extract.filter(*cloud_p); } //} Eigen::Vector3f lol_p (0.5f, 0, 0.5f); seg.setAxis(lol_p); while(cloud_p->points.size () > 0.1 * nr_points) { seg.setInputCloud (cloud_p); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { break; } else { /*std::cout << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << "\n";*/ pcl::ExtractIndices<pcl::PointXYZ> extract; extract.setInputCloud(cloud_p); extract.setIndices(inliers); extract.setNegative(true); extract.filter(*cloud_p); } } // Apply Statistical Noise Filter sor.setInputCloud (cloud_p); sor.filter (*cloud_p); if(cloud_p->points.size() > 0) { std::vector<pcl::PointIndices> cluster_indices; tree->setInputCloud (cloud_p); ec.setInputCloud (cloud_p); ec.extract (cluster_indices); std::cout << "Clusters detected: " << cluster_indices.size() << "\n"; // Convert to ROS data type } // Convert to ROS data type sensor_msgs::PointCloud2 output; pcl::toROSMsg(*cloud_p, output); // Publish the data downsample_pub.publish(output); }