int filterPlaneModel (pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud, float distance) { Eigen::VectorXf planecoeffs, coeffs; pcl::PointIndices::Ptr inliers (new pcl::PointIndices); pcl::SampleConsensusModelPlane<pcl::PointXYZRGB>::Ptr model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZRGB> (cloud)); pcl::ExtractIndices<pcl::PointXYZRGB> extract; pcl::RandomSampleConsensus<pcl::PointXYZRGB> ransac (model_p); ransac.setDistanceThreshold (distance); ransac.computeModel (); ransac.getInliers (inliers->indices); ransac.getModelCoefficients (planecoeffs); model_p->optimizeModelCoefficients (inliers->indices, planecoeffs, coeffs); model_p->selectWithinDistance (coeffs, distance, inliers->indices); if (inliers->indices.empty ()) return (-1); // Remove the points that fit a plane from the cloud pcl::ExtractIndices<pcl::PointXYZRGB> eifilter; eifilter.setInputCloud (cloud); eifilter.setIndices (inliers); eifilter.setNegative (true); eifilter.setKeepOrganized (true); eifilter.filter (*cloud); return (0); }
int main(int argc, char** argv) { // initialize PointClouds pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>); // populate our PointCloud with points cloud->width = 500; cloud->height = 1; cloud->is_dense = false; cloud->points.resize (cloud->width * cloud->height); for (size_t i = 0; i < cloud->points.size (); ++i) { if (pcl::console::find_argument (argc, argv, "-s") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0) { cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0); cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0); if (i % 5 == 0) cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0); else if(i % 2 == 0) cloud->points[i].z = sqrt( 1 - (cloud->points[i].x * cloud->points[i].x) - (cloud->points[i].y * cloud->points[i].y)); else cloud->points[i].z = - sqrt( 1 - (cloud->points[i].x * cloud->points[i].x) - (cloud->points[i].y * cloud->points[i].y)); } else { cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0); cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0); if( i % 2 == 0) cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0); else cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y); } } std::vector<int> inliers; // created RandomSampleConsensus object and compute the appropriated model pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud)); pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud)); if(pcl::console::find_argument (argc, argv, "-f") >= 0) { pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p); ransac.setDistanceThreshold (.01); ransac.computeModel(); ransac.getInliers(inliers); } else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 ) { pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s); ransac.setDistanceThreshold (.01); ransac.computeModel(); ransac.getInliers(inliers); } // copies all inliers of the model computed to another PointCloud pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final); // creates the visualization object and adds either our orignial cloud or all of the inliers // depending on the command line arguments specified. boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; if (pcl::console::find_argument (argc, argv, "-f") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0) viewer = simpleVis(final); else
int main(int argc, char** argv) { //check if number of arguments is proper if(argc!=3){ help(); exit(0); } // initialize PointClouds pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>); if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) == -1) //* load the file { cerr << "Couldn't read file :" << argv[1] << "\n"; return (-1); } std::vector<int> inliers; // created RandomSampleConsensus object and compute the appropriated model pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud)); pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud)); //RANSAC for Line model pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<PointT>()); if (pcl::console::find_argument(argc, argv, "-l") >= 0) { pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers_l(new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg; // Optional seg.setOptimizeCoefficients(true); // Mandatory seg.setModelType(pcl::SACMODEL_LINE); seg.setMethodType(pcl::SAC_RANSAC); seg.setDistanceThreshold(0.05); seg.setInputCloud(cloud); seg.segment(*inliers_l, *coefficients); // Write the line inliers to disk pcl::ExtractIndices<PointT> extract; extract.setInputCloud(cloud); extract.setIndices(inliers_l); extract.setNegative(false); extract.filter(*cloud_line); } if (pcl::console::find_argument(argc, argv, "-f") >= 0) { //RANSAC for Plane model pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p); ransac.setDistanceThreshold(.01); ransac.computeModel(); ransac.getInliers(inliers); } else if (pcl::console::find_argument(argc, argv, "-sf") >= 0) { //RANSAC for Sphere model pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s); ransac.setDistanceThreshold(.01); ransac.computeModel(); ransac.getInliers(inliers); } // copies all inliers of the model computed to another PointCloud pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final); // creates the visualization object and adds either our orignial cloud or all of the inliers // depending on the command line arguments specified. boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; if (pcl::console::find_argument(argc, argv, "-f") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0) { viewer = simpleVis(final); } else if (pcl::console::find_argument(argc, argv, "-l") >= 0) {