void depthPointsCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {

    // Instantiate various clouds
    pcl::PCLPointCloud2* cloud_intermediate = new pcl::PCLPointCloud2;
    pcl::PCLPointCloud2ConstPtr cloudPtr(cloud_intermediate);
    pcl::PointCloud<pcl::PointXYZ> cloud;

    // Convert to PCL data type
    pcl_conversions::toPCL(*cloud_msg, *cloud_intermediate);

    // Apply Voxel Filter on PCLPointCloud2
    vox.setInputCloud (cloudPtr);
    vox.setInputCloud (cloudPtr);
    vox.filter (*cloud_intermediate);

    // Convert PCL::PointCloud2 to PCL::PointCloud<PointXYZ>
    pcl::fromPCLPointCloud2(*cloud_intermediate, cloud);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p = cloud.makeShared();

    // Apply Passthrough Filter
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0.3, 1);
    pass.setInputCloud (cloud_p);
    //pass.setFilterLimitsNegative (true);
    pass.filter (*cloud_p);

    // Apply Passthrough Filter
    pass.setFilterFieldName ("x");
    pass.setFilterLimits (-0.2, 0.2);
    pass.setInputCloud (cloud_p);
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0.0, 3.0);
    //pass.setFilterLimitsNegative (true);
    pass.filter (*cloud_p);

    // Apply Statistical Noise Filter
    sor.setInputCloud (cloud_p);
    sor.filter (*cloud_p);

    // Planar segmentation: Remove large planes? Or extract floor?
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);

    int nr_points = (int) cloud_p->points.size ();

    Eigen::Vector3f lol (0, 1, 0);
    seg.setEpsAngle(  30.0f * (3.14f/180.0f) );
    seg.setAxis(lol);
    //while(cloud_p->points.size () > 0.2 * nr_points) {
    sor.setInputCloud (cloud_p);
    sor.filter (*cloud_p);
    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    // Optional
    seg.setOptimizeCoefficients (true);
    // Mandatory
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (0.01);
    seg.setInputCloud (cloud_p);
    seg.segment (*inliers, *coefficients);

    if (inliers->indices.size () == 0)
    {
        //break;
    }
    else {
        /*std::cout << "Model coefficients: " << coefficients->values[0] << " "
                      << coefficients->values[1] << " "
                      << coefficients->values[2] << " "
                      << coefficients->values[3] << "\n";*/
        pcl::ExtractIndices<pcl::PointXYZ> extract;
        extract.setInputCloud(cloud_p);
        extract.setIndices(inliers);
        extract.setNegative(true);
        extract.filter(*cloud_p);
    }
    //}

    Eigen::Vector3f lol_p (0.5f, 0, 0.5f);
    seg.setAxis(lol_p);
    while(cloud_p->points.size () > 0.1 * nr_points) {

        seg.setInputCloud (cloud_p);
        seg.segment (*inliers, *coefficients);

        if (inliers->indices.size () == 0)
        {
            break;
        }
        else {
            /*std::cout << "Model coefficients: " << coefficients->values[0] << " "
                      << coefficients->values[1] << " "
                      << coefficients->values[2] << " "
                      << coefficients->values[3] << "\n";*/
            pcl::ExtractIndices<pcl::PointXYZ> extract;
            extract.setInputCloud(cloud_p);
            extract.setIndices(inliers);
            extract.setNegative(true);
            extract.filter(*cloud_p);
        }
    }

    // Apply Statistical Noise Filter
    sor.setInputCloud (cloud_p);
    sor.filter (*cloud_p);

    if(cloud_p->points.size() > 0) {
        std::vector<pcl::PointIndices> cluster_indices;
        tree->setInputCloud (cloud_p);
        ec.setInputCloud (cloud_p);
        ec.extract (cluster_indices);

        std::cout << "Clusters detected: " << cluster_indices.size() << "\n";
        // Convert to ROS data type
    }
    // Convert to ROS data type
    sensor_msgs::PointCloud2 output;
    pcl::toROSMsg(*cloud_p, output);


    // Publish the data
    downsample_pub.publish(output);

}
Ejemplo n.º 2
0
    bool compute_object(const int i, std::vector<pcl::PointIndices > &segments) {
        // Call get_plane, get_cylinder, get_sphere
//        float plane_score = compute_plane(); // plane_inliers_ has the pc.
        // float cylinder_score = compute_cylinder(); // cylinder_inliers_ has the pc

        // Select the lowest score and store it in object_inliers_.
        // inliers_object_ = (plane_score > cylinder_score ? inliers_plane_ : inliers_cylinder_);

        inliers_object_ = inliers_plane_;
        // inliers_object_ = inliers_cylinder_;

        // Test if the point cloud is too small
        if (inliers_object_->indices.size() < CUT_THRESHOLD) {
            std::cerr << "object inliers has " << inliers_object_->indices.size()
                    << " < " << CUT_THRESHOLD << " points. Aborting..." << std::endl;
            return false;
        }

        /*
        // Debugging info
        pcl::PointCloud<Point>::Ptr best_object_(new pcl::PointCloud<Point>);
        pcl::copyPointCloud(*cloud_filtered, *inliers_object_, *best_object_);
        std::stringstream debug_s0;
        debug_s0 << "no_cluster_object_" << i << ".pcd";
        pcl::io::savePCDFile(debug_s0.str(), *best_object_);
         */

        // Euclidean Clustering
        std::vector<pcl::PointIndices> clusters;
        cluster_.setInputCloud(cloud_filtered);
        cluster_.setIndices(inliers_object_);
        cluster_.extract(clusters);
        std::cout << "Number of Euclidian clusters found: " << clusters.size() << std::endl;

        // Check if there is no clusters
        if (clusters.size() == 0) {
            std::cerr << "No clusters found. Aborting..." << std::endl;
            return false;
        }

        main_cluster_.reset(new pcl::PointIndices(clusters.at(0)));

        // Extract the inliers from the input cloud
        extract.setInputCloud(cloud_filtered);
        extract.setIndices(main_cluster_);
        extract.setNegative(false);

        pcl::PointCloud<Point>::Ptr cloud_plane(new pcl::PointCloud<Point > ());
        extract.filter(*cloud_plane);

        // Test if cluster[0] is too small
        if (main_cluster_->indices.size() < CUT_THRESHOLD) {
            std::cerr << "object cluster[0] has " << main_cluster_->indices.size()
                    << " < " << CUT_THRESHOLD << " points. Not writing to disk..." << std::endl;
        } else {
            // Write the inliers to disk
            std::stringstream debug_s;
            if (inliers_object_ == inliers_cylinder_) {
                debug_s << "cylinder_object_" << i << ".pcd";
            } else if (inliers_object_ == inliers_plane_) {
                debug_s << "plane_object_" << i << ".pcd";
            } else {
                debug_s << "unknown_object_" << i << ".pcd";
            }
            pcl::io::savePCDFile(debug_s.str(), *cloud_plane);
            std::cout << "PointCloud representing the extracted component: " <<
                    cloud_plane->points.size() << " data points." << std::endl;

            // Insert the points to segments for cfg3D
            pcl::PointIndices::Ptr originalIndices(new pcl::PointIndices());
            originalIndices->indices.resize(cloud_plane->size());

            for (unsigned int j = 0; j < originalIndices->indices.size(); j++) {
                originalIndices->indices.at(j) = cloud_plane->points.at(j).rgba;
            }
            segments.push_back(*originalIndices);


            // Debugging for cfg3D
            pcl::PointCloud<Point>::Ptr cfg_segment_cloud(new pcl::PointCloud<Point > ());

            pcl::ExtractIndices<Point> cfg_extract;
            cfg_extract.setNegative(false);
            cfg_extract.setInputCloud(cloud);
            cfg_extract.setIndices(originalIndices);
            cfg_extract.filter(*cfg_segment_cloud);

            /*
            cfg_segment_cloud->points.resize(originalIndices->indices.size());
            for (unsigned int j = 0; j < originalIndices->indices.size(); j++) {
                unsigned int index = originalIndices->indices.at(j);
                cfg_segment_cloud->points.at(j).x = cloud->points.at(index).x;
                cfg_segment_cloud->points.at(j).y = cloud->points.at(index).y;
                cfg_segment_cloud->points.at(j).z = cloud->points.at(index).z;
            }
             */
            std::stringstream cfg_debug_s;
            cfg_debug_s << "cfg_debug_segment_" << i << ".pcd";
            pcl::io::savePCDFile(cfg_debug_s.str(), *cfg_segment_cloud);

        }

        // Remove cluster[0], update cloud_filtered and cloud_normals
        extract.setNegative(true);
        extract.filter(*cloud_filtered);
        extract_normals.setNegative(true);
        extract_normals.setInputCloud(cloud_normals);
        extract_normals.setIndices(main_cluster_);
        extract_normals.filter(*cloud_normals);

        return true;
    }
Ejemplo n.º 3
0
    bool compute_object(const int i) {
        // Call get_plane, get_cylinder, get_sphere
        float plane_score = compute_plane(); // plane_inliers_ has the pc.
        float cylinder_score = compute_cylinder(); // cylinder_inliers_ has the pc

        // Select the lowest score and store it in object_inliers_.
        inliers_object_ = (plane_score > cylinder_score ? inliers_plane_ : inliers_cylinder_);

        // inliers_object_ = inliers_plane_;
        // inliers_object_ = inliers_cylinder_;

        // Test if the point cloud is too small
        if (inliers_object_->indices.size() < CUT_THRESHOLD) {
            std::cerr << "object inliers has "<< inliers_object_->indices.size()
                    << " < " << CUT_THRESHOLD <<" points. Aborting..." << std::endl;
            return false;
        }

        /*
        // Debugging info
        pcl::PointCloud<Point>::Ptr best_object_(new pcl::PointCloud<Point>);
        pcl::copyPointCloud(*cloud_filtered, *inliers_object_, *best_object_);
        std::stringstream debug_s0;
        debug_s0 << "no_cluster_object_" << i << ".pcd";
        pcl::io::savePCDFile(debug_s0.str(), *best_object_);
        */

        // Euclidean Clustering
        std::vector<pcl::PointIndices> clusters;
        cluster_.setInputCloud(cloud_filtered);
        cluster_.setIndices(inliers_object_);
        cluster_.extract(clusters);
        std::cout << "Number of Euclidian clusters found: " << clusters.size() << std::endl;

        // Check if there is no clusters
        if (clusters.size() == 0) {
            std::cerr << "No clusters found. Aborting..." << std::endl;
            return false;
        }

        main_cluster_.reset(new pcl::PointIndices(clusters.at(0)));

        // Extract the inliers from the input cloud
        extract.setInputCloud(cloud_filtered);
        extract.setIndices(main_cluster_);
        extract.setNegative(false);

        pcl::PointCloud<Point>::Ptr cloud_plane(new pcl::PointCloud<Point > ());
        extract.filter(*cloud_plane);

        // Test if cluster[0] is too small
        if (main_cluster_->indices.size() < CUT_THRESHOLD) {
            std::cerr << "object cluster[0] has "<< main_cluster_->indices.size()
                    << " < " << CUT_THRESHOLD <<" points. Not writing to disk..." << std::endl;
        } else {
          // Write the inliers to disk
          std::stringstream debug_s;
          if (inliers_object_ == inliers_cylinder_) {
            debug_s << "cylinder_object_" << i << ".pcd";
          } else if (inliers_object_ == inliers_plane_) {
            debug_s << "plane_object_" << i << ".pcd";
          } else {
            debug_s << "unknown_object_" << i << ".pcd";
          }
          pcl::io::savePCDFile(debug_s.str(), *cloud_plane);
          std::cout << "PointCloud representing the extracted component: " <<
            cloud_plane->points.size() << " data points." << std::endl;
        }

        // Remove cluster[0], update cloud_filtered and cloud_normals
        extract.setNegative(true);
        extract.filter(*cloud_filtered);
        extract_normals.setNegative(true);
        extract_normals.setInputCloud(cloud_normals);
        extract_normals.setIndices(main_cluster_);
        extract_normals.filter(*cloud_normals);

        return true;
    }