00001 #include <iostream> 00002 #include <pcl/ModelCoefficients.h> 00003 #include <pcl/io/pcd_io.h> 00004 #include <pcl/point_types.h> 00005 #include <pcl/sample_consensus/method_types.h> 00006 #include <pcl/sample_consensus/model_types.h> 00007 #include <pcl/segmentation/sac_segmentation.h> 00008 00009 int 00010 main (int argc, char** argv) 00011 { 00012 pcl::PointCloud<pcl::PointXYZ> cloud; 00013 00014 // Fill in the cloud data 00015 cloud.width = 15; 00016 cloud.height = 1; 00017 cloud.points.resize (cloud.width * cloud.height); 00018 00019 // Generate the data 00020 for (size_t i = 0; i < cloud.points.size (); ++i) 00021 { 00022 cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f); 00023 cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f); 00024 cloud.points[i].z = 1.0; 00025 } 00026 00027 // Set a few outliers 00028 cloud.points[0].z = 2.0; 00029 cloud.points[3].z = -2.0; 00030 cloud.points[6].z = 4.0; 00031 00032 std::cerr << "Point cloud data: " << cloud.points.size () << " points" << std::endl; 00033 for (size_t i = 0; i < cloud.points.size (); ++i) 00034 std::cerr << " " << cloud.points[i].x << " " 00035 << cloud.points[i].y << " " 00036 << cloud.points[i].z << std::endl; 00037 00038 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); 00039 pcl::PointIndices::Ptr inliers (new pcl::PointIndices); 00040 // Create the segmentation object 00041 pcl::SACSegmentation<pcl::PointXYZ> seg; 00042 // Optional 00043 seg.setOptimizeCoefficients (true); 00044 // Mandatory 00045 seg.setModelType (pcl::SACMODEL_PLANE); 00046 seg.setMethodType (pcl::SAC_RANSAC); 00047 seg.setDistanceThreshold (0.01); 00048 00049 seg.setInputCloud (cloud.makeShared ()); 00050 seg.segment (*inliers, *coefficients); 00051 00052 if (inliers->indices.size () == 0) 00053 { 00054 PCL_ERROR ("Could not estimate a planar model for the given dataset."); 00055 return (-1); 00056 } 00057 00058 std::cerr << "Model coefficients: " << coefficients->values[0] << " " 00059 << coefficients->values[1] << " " 00060 << coefficients->values[2] << " " 00061 << coefficients->values[3] << std::endl; 00062 00063 std::cerr << "Model inliers: " << inliers->indices.size () << std::endl; 00064 for (size_t i = 0; i < inliers->indices.size (); ++i) 00065 std::cerr << inliers->indices[i] << " " << cloud.points[inliers->indices[i]].x << " " 00066 << cloud.points[inliers->indices[i]].y << " " 00067 << cloud.points[inliers->indices[i]].z << std::endl; 00068 00069 return (0); 00070 }