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; }
Extractor() { tree.reset(new pcl::KdTreeFLANN<Point > ()); cloud.reset(new pcl::PointCloud<Point>); cloud_filtered.reset(new pcl::PointCloud<Point>); cloud_normals.reset(new pcl::PointCloud<pcl::Normal>); coefficients_plane_.reset(new pcl::ModelCoefficients); coefficients_cylinder_.reset(new pcl::ModelCoefficients); inliers_plane_.reset(new pcl::PointIndices); inliers_cylinder_.reset(new pcl::PointIndices); // Filter Pass pass.setFilterFieldName("z"); pass.setFilterLimits(-100, 100); // VoxelGrid for Downsampling LEAFSIZE = 0.01f; vg.setLeafSize(LEAFSIZE, LEAFSIZE, LEAFSIZE); // Any object < CUT_THRESHOLD will be abandoned. //CUT_THRESHOLD = (int) (LEAFSIZE * LEAFSIZE * 7000000); // 700 CUT_THRESHOLD = 700; //for nonfiltering // Clustering cluster_.setClusterTolerance(0.06 * UNIT); cluster_.setMinClusterSize(50.0); cluster_.setSearchMethod(clusters_tree_); // Normals ne.setSearchMethod(tree); ne.setKSearch(50); // 50 by default // plane SAC seg_plane.setOptimizeCoefficients(true); seg_plane.setModelType(pcl::SACMODEL_NORMAL_PLANE); seg_plane.setNormalDistanceWeight(0.1); // 0.1 seg_plane.setMethodType(pcl::SAC_RANSAC); seg_plane.setMaxIterations(1000); // 10000 seg_plane.setDistanceThreshold(0.05); // 0.03 // cylinder SAC seg_cylinder.setOptimizeCoefficients(true); seg_cylinder.setModelType(pcl::SACMODEL_CYLINDER); seg_cylinder.setMethodType(pcl::SAC_RANSAC); seg_cylinder.setNormalDistanceWeight(0.1); seg_cylinder.setMaxIterations(10000); seg_cylinder.setDistanceThreshold(0.02); // 0.05 seg_cylinder.setRadiusLimits(0.02, 0.07); // [0, 0.1] }