random_sample_consensus.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <pcl/console/parse.h>
00003 #include <pcl/filters/extract_indices.h>
00004 #include <pcl/io/pcd_io.h>
00005 #include <pcl/point_types.h>
00006 #include <pcl/sample_consensus/ransac.h>
00007 #include <pcl/sample_consensus/sac_model_plane.h>
00008 #include <pcl/sample_consensus/sac_model_sphere.h>
00009 #include <pcl/visualization/pcl_visualizer.h>
00010 #include <boost/thread/thread.hpp>
00011 
00012 boost::shared_ptr<pcl::visualization::PCLVisualizer>
00013 simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
00014 {
00015   // --------------------------------------------
00016   // -----Open 3D viewer and add point cloud-----
00017   // --------------------------------------------
00018   boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
00019   viewer->setBackgroundColor (0, 0, 0);
00020   viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
00021   viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
00022   //viewer->addCoordinateSystem (1.0);
00023   viewer->initCameraParameters ();
00024   return (viewer);
00025 }
00026 
00027 int
00028 main(int argc, char** argv)
00029 {
00030   // initialize PointClouds
00031   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00032   pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
00033 
00034   // populate our PointCloud with points
00035   cloud->width    = 500;
00036   cloud->height   = 1;
00037   cloud->is_dense = false;
00038   cloud->points.resize (cloud->width * cloud->height);
00039   for (size_t i = 0; i < cloud->points.size (); ++i)
00040   {
00041     if (pcl::console::find_argument (argc, argv, "-s") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
00042     {
00043       cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
00044       cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
00045       if (i % 5 == 0)
00046         cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
00047       else if(i % 2 == 0)
00048         cloud->points[i].z =  sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
00049                                       - (cloud->points[i].y * cloud->points[i].y));
00050       else
00051         cloud->points[i].z =  - sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
00052                                         - (cloud->points[i].y * cloud->points[i].y));
00053     }
00054     else
00055     {
00056       cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
00057       cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
00058       if( i % 2 == 0)
00059         cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
00060       else
00061         cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
00062     }
00063   }
00064 
00065   std::vector<int> inliers;
00066 
00067   // created RandomSampleConsensus object and compute the appropriated model
00068   pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
00069     model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud));
00070   pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
00071     model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));
00072   if(pcl::console::find_argument (argc, argv, "-f") >= 0)
00073   {
00074     pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
00075     ransac.setDistanceThreshold (.01);
00076     ransac.computeModel();
00077     ransac.getInliers(inliers);
00078   }
00079   else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 )
00080   {
00081     pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
00082     ransac.setDistanceThreshold (.01);
00083     ransac.computeModel();
00084     ransac.getInliers(inliers);
00085   }
00086 
00087   // copies all inliers of the model computed to another PointCloud
00088   pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
00089 
00090   // creates the visualization object and adds either our orignial cloud or all of the inliers
00091   // depending on the command line arguments specified.
00092   boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
00093   if (pcl::console::find_argument (argc, argv, "-f") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
00094     viewer = simpleVis(final);
00095   else
00096     viewer = simpleVis(cloud);
00097   while (!viewer->wasStopped ())
00098   {
00099     viewer->spinOnce (100);
00100     boost::this_thread::sleep (boost::posix_time::microseconds (100000));
00101   }
00102   return 0;
00103  }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:32:00