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;
}
Esempio n. 2
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]
    }