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) {
