コード例 #1
0
    bool
    segmentCloud (pointing_object_evaluation::ObjectSegmentationRequest::Request &req, pointing_object_evaluation::ObjectSegmentationRequest::Response &res)
    {
      CloudPtr cloud (new Cloud ());
      {
        //boost::lock_guard<boost::mutex> lock (cloud_mutex_);
        *cloud = *cloud_;
      }
      
      unsigned char red [6] = {255,   0,   0, 255, 255,   0};
      unsigned char grn [6] = {  0, 255,   0, 255,   0, 255};
      unsigned char blu [6] = {  0,   0, 255,   0, 255, 255};
      pcl::PointCloud<pcl::PointXYZRGB> aggregate_cloud;

      // Estimate Normals
      double ne_start = pcl::getTime ();
      pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
      ne_.setInputCloud (cloud);
      ne_.compute (*normal_cloud);
      double ne_end = pcl::getTime ();
      //std::cout << "NE took: " << double(ne_end - ne_start) << std::endl;
      
      // Segment Planes
      double mps_start = pcl::getTime ();
      std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
      std::vector<pcl::ModelCoefficients> model_coefficients;
      std::vector<pcl::PointIndices> inlier_indices;  
      pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
      std::vector<pcl::PointIndices> label_indices;
      std::vector<pcl::PointIndices> boundary_indices;
      mps_.setInputNormals (normal_cloud);
      mps_.setInputCloud (cloud);
      //mps.segment (regions);
      mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);      
      
      //Segment Objects
      typename pcl::PointCloud<PointT>::CloudVectorType clusters;

      if (regions.size () > 0)
      {
        //printf ("got some planes!\n");
        std::vector<bool> plane_labels;
        plane_labels.resize (label_indices.size (), false);
        for (size_t i = 0; i < label_indices.size (); i++)
        {
          if (label_indices[i].indices.size () > 10000)
          {
            plane_labels[i] = true;
          }
        }  
        //printf ("made label vec\n");
        
        typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator_ (new typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>());
        euclidean_cluster_comparator_->setInputCloud (cloud);
        euclidean_cluster_comparator_->setLabels (labels);
        euclidean_cluster_comparator_->setExcludeLabels (plane_labels);
        euclidean_cluster_comparator_->setDistanceThreshold (0.01f, false);
        

        pcl::PointCloud<pcl::Label> euclidean_labels;
        std::vector<pcl::PointIndices> euclidean_label_indices;
        pcl::OrganizedConnectedComponentSegmentation<PointT,pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_);
        euclidean_segmentation.setInputCloud (cloud);
        euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
        
        //printf ("done segmenting the clusters\n");
        
        std::vector<Eigen::Vector4f> centroids;
        
        for (size_t i = 0; i < euclidean_label_indices.size (); i++)
        {
          if (euclidean_label_indices[i].indices.size () > 1000)
          {
            pcl::PointCloud<PointT> cluster;
            pcl::copyPointCloud (*cloud,euclidean_label_indices[i].indices,cluster);	    
            clusters.push_back (cluster);
            
	    /*Eigen::Vector4f centroid;
            pcl::compute3DCentroid (cluster, centroid);
	    centroids.push_back (centroid);*/
            
	    //pcl::PointXYZ centroid_pt (centroid[0], centroid[1], centroid[2]);
            
            
            //double dist = sqrt (centroid[0]*centroid[0] + centroid[1]*centroid[1] + centroid[2]*centroid[2]);
            //object_points_.push_back (centroid_pt);

	    
            // Send to RViz
            pcl::PointCloud<pcl::PointXYZRGB> color_cluster;
            pcl::copyPointCloud (cluster, color_cluster);
            for (size_t j = 0; j < color_cluster.size (); j++)
            {
              color_cluster.points[j].r = (color_cluster.points[j].r + red[i%6]) / 2;
              color_cluster.points[j].g = (color_cluster.points[j].g + grn[i%6]) / 2;
              color_cluster.points[j].b = (color_cluster.points[j].b + blu[i%6]) / 2;
            }
            aggregate_cloud += color_cluster;
	    
          }    
        }
        
        PCL_INFO ("Got %d euclidean clusters!\n", clusters.size ());
	//PCL_INFO ("Got %d centroids!\n", centroids.size ());

        aggregate_cloud.header = cloud->header;
        sensor_msgs::PointCloud2 cloud_msg;
        pcl::toROSMsg (aggregate_cloud, cloud_msg);
        object_cloud_pub_.publish (cloud_msg);

        // TODO: mutex
        
	/*clusters_ = clusters;
	  centroids_= centroids;*/
      }
      
    }
コード例 #2
0
ファイル: multiplane_segmenter.cpp プロジェクト: Cerarus/v4r
void
MultiplaneSegmenter<PointT>::segment()
{
    clusters_.clear();
    computeTablePlanes();


    typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr
            euclidean_cluster_comparator_ (new pcl::EuclideanClusterComparator< PointT, pcl::Normal,pcl::Label> ());

    pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
    labels->points.resize( scene_->points.size() );
    std::vector < pcl::PointIndices > label_indices(2);
    std::vector<bool> plane_labels;
    plane_labels.resize (label_indices.size (), false);

    if ( !all_planes_.empty() )
    {
        size_t table_plane_selected = 0;
        size_t max_inliers_found = 0;
        std::vector<size_t> plane_inliers_counts (all_planes_.size (), 0);

        for (size_t i = 0; i < all_planes_.size (); i++)
        {
            const Eigen::Vector3f plane_normal = all_planes_[i]->coefficients_.head(3);
            for (size_t j = 0; j < scene_->points.size (); j++)
            {
                if ( !pcl::isFinite( scene_->points[j] ) )
                    continue;

                const Eigen::Vector3f &xyz_p = scene_->points[j].getVector3fMap ();

                if (std::abs ( xyz_p.dot(plane_normal) ) < param_.sensor_noise_max_)
                    plane_inliers_counts[i]++;
            }

            if ( plane_inliers_counts[i] > max_inliers_found )
            {
                table_plane_selected = i;
                max_inliers_found = plane_inliers_counts[i];
            }
        }

        size_t itt = table_plane_selected;
        dominant_plane_ = all_planes_[itt]->coefficients_;
        Eigen::Vector3f normal_table = all_planes_[itt]->coefficients_.head(3);
        size_t inliers_count_best = plane_inliers_counts[itt];

        //check that the other planes with similar normal are not higher than the table_plane_selected
        for (size_t i = 0; i < all_planes_.size (); i++)
        {
            Eigen::Vector4f model = all_planes_[i]->coefficients_;
            Eigen::Vector3f normal_tmp = all_planes_[i]->coefficients_.head(3);

            int inliers_count = plane_inliers_counts[i];

            if ((normal_tmp.dot (normal_table) > 0.95) && (inliers_count_best * 0.5 <= inliers_count))
            {
                //check if this plane is higher, projecting a point on the normal direction
                std::cout << model[3] << " " << dominant_plane_[3] << std::endl;
                if (model[3] < dominant_plane_[3])
                {
                    PCL_WARN ("Changing table plane...");
                    dominant_plane_ = all_planes_[i]->coefficients_;
                    normal_table = normal_tmp;
                    inliers_count_best = inliers_count;
                }
            }
        }

        dominant_plane_ = all_planes_[table_plane_selected]->coefficients_;

        //create two labels, 1 one for points belonging to or under the plane, 1 for points above the plane

        for (size_t j = 0; j < scene_->points.size (); j++)
        {
            const Eigen::Vector3f &xyz_p = scene_->points[j].getVector3fMap ();

            if (! pcl::isFinite(scene_->points[j]) )
                continue;

            float val = xyz_p[0] * dominant_plane_[0] + xyz_p[1] * dominant_plane_[1] + xyz_p[2] * dominant_plane_[2] + dominant_plane_[3];

            if (val >= param_.sensor_noise_max_)
            {
                labels->points[j].label = 1;
                label_indices[0].indices.push_back (j);
            }
            else
            {
                labels->points[j].label = 0;
                label_indices[1].indices.push_back (j);
            }
        }

        plane_labels[0] = true;
    }
    else
    {
        for (size_t j = 0; j < scene_->points.size (); j++)
            labels->points[j].label = 1;
    }

    euclidean_cluster_comparator_->setInputCloud (scene_);
    euclidean_cluster_comparator_->setLabels (labels);
    euclidean_cluster_comparator_->setExcludeLabels (plane_labels);
    euclidean_cluster_comparator_->setDistanceThreshold ( param_.min_distance_between_clusters_, true);

    pcl::PointCloud < pcl::Label > euclidean_labels;
    std::vector < pcl::PointIndices > euclidean_label_indices;
    pcl::OrganizedConnectedComponentSegmentation<PointT, pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_);
    euclidean_segmentation.setInputCloud (scene_);
    euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);

    for (size_t i = 0; i < euclidean_label_indices.size (); i++)
    {
        if ( euclidean_label_indices[i].indices.size () >= param_.min_cluster_size_)
        {
            clusters_.push_back (euclidean_label_indices[i]);
        }
    }
}
コード例 #3
0
void
OrganizedMultiplaneSegmenter<PointT>::segment()
{
    clusters_.clear();
    pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
    mps.setMinInliers (param_.num_plane_inliers_);
    mps.setAngularThreshold (param_.angular_threshold_deg_ * M_PI/180.f);
    mps.setDistanceThreshold (param_.sensor_noise_max_);
    mps.setInputNormals (normals_);
    mps.setInputCloud (scene_);

    std::vector < pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
    std::vector < pcl::ModelCoefficients > model_coeff;
    std::vector < pcl::PointIndices > inlier_indices;
    pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
    std::vector < pcl::PointIndices > label_indices;
    std::vector < pcl::PointIndices > boundary_indices;

    typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr ref_comp (
                new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ());
    ref_comp->setDistanceThreshold (param_.sensor_noise_max_, false);
    ref_comp->setAngularThreshold (2 * M_PI/180.f);
    mps.setRefinementComparator (ref_comp);
    mps.segmentAndRefine (regions, model_coeff, inlier_indices, labels, label_indices, boundary_indices);

    std::cout << "Number of planes found:" << model_coeff.size () << std::endl;
    if ( !model_coeff.size() )
        return;

    size_t table_plane_selected = 0;
    int max_inliers_found = -1;
    std::vector<size_t> plane_inliers_counts;
    plane_inliers_counts.resize (model_coeff.size ());

    for (size_t i = 0; i < model_coeff.size (); i++)
    {
        Eigen::Vector4f table_plane = Eigen::Vector4f (model_coeff[i].values[0], model_coeff[i].values[1],
                model_coeff[i].values[2], model_coeff[i].values[3]);

        std::cout << "Number of inliers for this plane:" << inlier_indices[i].indices.size () << std::endl;
        size_t remaining_points = 0;
        typename pcl::PointCloud<PointT>::Ptr plane_points (new pcl::PointCloud<PointT> (*scene_));
        for (size_t j = 0; j < plane_points->points.size (); j++)
        {
            const Eigen::Vector3f xyz_p = plane_points->points[j].getVector3fMap ();

            if ( !pcl::isFinite( plane_points->points[j] ) )
                continue;

            float val = xyz_p[0] * table_plane[0] + xyz_p[1] * table_plane[1] + xyz_p[2] * table_plane[2] + table_plane[3];

            if (std::abs (val) > param_.sensor_noise_max_)
            {
                plane_points->points[j].x = std::numeric_limits<float>::quiet_NaN ();
                plane_points->points[j].y = std::numeric_limits<float>::quiet_NaN ();
                plane_points->points[j].z = std::numeric_limits<float>::quiet_NaN ();
            }
            else
                remaining_points++;
        }

        plane_inliers_counts[i] = remaining_points;

        if ( (int)remaining_points > max_inliers_found )
        {
            table_plane_selected = i;
            max_inliers_found = remaining_points;
        }
    }

    size_t itt = table_plane_selected;
    dominant_plane_ = Eigen::Vector4f (model_coeff[itt].values[0], model_coeff[itt].values[1], model_coeff[itt].values[2], model_coeff[itt].values[3]);
    Eigen::Vector3f normal_table = Eigen::Vector3f (model_coeff[itt].values[0], model_coeff[itt].values[1], model_coeff[itt].values[2]);

    size_t inliers_count_best = plane_inliers_counts[itt];

    //check that the other planes with similar normal are not higher than the table_plane_selected
    for (size_t i = 0; i < model_coeff.size (); i++)
    {
        Eigen::Vector4f model = Eigen::Vector4f (model_coeff[i].values[0], model_coeff[i].values[1], model_coeff[i].values[2],
                model_coeff[i].values[3]);

        Eigen::Vector3f normal = Eigen::Vector3f (model_coeff[i].values[0], model_coeff[i].values[1], model_coeff[i].values[2]);

        int inliers_count = plane_inliers_counts[i];

        std::cout << "Dot product is:" << normal.dot (normal_table) << std::endl;
        if ((normal.dot (normal_table) > 0.95) && (inliers_count_best * 0.5 <= inliers_count))
        {
            //check if this plane is higher, projecting a point on the normal direction
            std::cout << "Check if plane is higher, then change table plane" << std::endl;
            std::cout << model[3] << " " << dominant_plane_[3] << std::endl;
            if (model[3] < dominant_plane_[3])
            {
                PCL_WARN ("Changing table plane...");
                table_plane_selected = i;
                dominant_plane_ = model;
                normal_table = normal;
                inliers_count_best = inliers_count;
            }
        }
    }

    dominant_plane_ = Eigen::Vector4f (model_coeff[table_plane_selected].values[0], model_coeff[table_plane_selected].values[1],
            model_coeff[table_plane_selected].values[2], model_coeff[table_plane_selected].values[3]);

    typename pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr
            euclidean_cluster_comparator_ (new pcl::EuclideanClusterComparator< PointT, pcl::Normal,pcl::Label> ());

    //create two labels, 1 one for points belonging to or under the plane, 1 for points above the plane
    label_indices.resize (2);

    for (size_t j = 0; j < scene_->points.size (); j++)
    {
        Eigen::Vector3f xyz_p = scene_->points[j].getVector3fMap ();

        if (! pcl::isFinite(scene_->points[j]) )
            continue;

        float val = xyz_p[0] * dominant_plane_[0] + xyz_p[1] * dominant_plane_[1] + xyz_p[2] * dominant_plane_[2] + dominant_plane_[3];

        if (val >= param_.sensor_noise_max_)
        {
            /*plane_points->points[j].x = std::numeric_limits<float>::quiet_NaN ();
     plane_points->points[j].y = std::numeric_limits<float>::quiet_NaN ();
     plane_points->points[j].z = std::numeric_limits<float>::quiet_NaN ();*/
            labels->points[j].label = 1;
            label_indices[0].indices.push_back (j);
        }
        else
        {
            labels->points[j].label = 0;
            label_indices[1].indices.push_back (j);
        }
    }

    std::vector<bool> plane_labels;
    plane_labels.resize (label_indices.size (), false);
    plane_labels[0] = true;

    euclidean_cluster_comparator_->setInputCloud (scene_);
    euclidean_cluster_comparator_->setLabels (labels);
    euclidean_cluster_comparator_->setExcludeLabels (plane_labels);
    euclidean_cluster_comparator_->setDistanceThreshold (0.035f, true);

    pcl::PointCloud < pcl::Label > euclidean_labels;
    std::vector < pcl::PointIndices > euclidean_label_indices;
    pcl::OrganizedConnectedComponentSegmentation<PointT, pcl::Label> euclidean_segmentation (euclidean_cluster_comparator_);
    euclidean_segmentation.setInputCloud (scene_);
    euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);

    for (size_t i = 0; i < euclidean_label_indices.size (); i++)
    {
        if ( (int)euclidean_label_indices[i].indices.size () >= param_.min_cluster_size_)
        {
            clusters_.push_back (euclidean_label_indices[i]);
        }
    }
}