示例#1
0
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]);
        }
    }
}
int main (int, char**)
{
  Point_set pts;

  pts.add_normal_map();
  
  bool map_added = false;
  Size_t_map echo_map;
  Color_map color_map;

  boost::tie (echo_map, map_added) = pts.add_property_map<std::size_t> ("echo");
  CGAL_assertion (map_added);
  boost::tie (color_map, map_added) = pts.add_property_map<Classification::RGB_Color> ("color");
  CGAL_assertion (map_added);

  for (std::size_t i = 0; i < 1000; ++ i)
  {
    Point_set::iterator it
      = pts.insert (Point (CGAL::get_default_random().get_double(),
                           CGAL::get_default_random().get_double(),
                           CGAL::get_default_random().get_double()),
                    Vector (CGAL::get_default_random().get_double(),
                            CGAL::get_default_random().get_double(),
                            CGAL::get_default_random().get_double()));
    echo_map[*it] = std::size_t(CGAL::get_default_random().get_int(0, 4));
    color_map[*it] = CGAL::make_array ((unsigned char)(CGAL::get_default_random().get_int(0, 255)),
                                       (unsigned char)(CGAL::get_default_random().get_int(0, 255)),
                                       (unsigned char)(CGAL::get_default_random().get_int(0, 255)));
  }

  Feature_set features;
  Feature_generator generator (features, pts, pts.point_map(),
                               5,  // using 5 scales
                               pts.normal_map(),
                               color_map, echo_map);

  CGAL_assertion (generator.number_of_scales() == 5);
  CGAL_assertion (features.size() == 80);

  Label_set labels;

  std::vector<int> training_set (pts.size(), -1);
  for (std::size_t i = 0; i < 20; ++ i)
  {
    std::ostringstream oss;
    oss << "label_" << i;
    Label_handle lh = labels.add(oss.str().c_str());

    for (std::size_t j = 0; j < 10; ++ j)
      training_set[std::size_t(CGAL::get_default_random().get_int(0, int(training_set.size())))] = int(i);
  }
  CGAL_assertion (labels.size() == 20);
  
  Classifier classifier (labels, features);
  
  classifier.train<CGAL::Sequential_tag> (training_set, 800);
#ifdef CGAL_LINKED_WITH_TBB
  classifier.train<CGAL::Parallel_tag> (training_set, 800);
#endif

  std::vector<int> label_indices(pts.size(), -1);
  
  Classification::classify<CGAL::Sequential_tag>
    (pts, labels, classifier, label_indices);

  Classification::classify_with_local_smoothing<CGAL::Sequential_tag>
    (pts, pts.point_map(), labels, classifier,
     generator.neighborhood().sphere_neighbor_query(0.01f),
     label_indices);

  Classification::classify_with_graphcut<CGAL::Sequential_tag>
    (pts, pts.point_map(), labels, classifier,
     generator.neighborhood().k_neighbor_query(12),
     0.2f, 10, label_indices);

#ifdef CGAL_LINKED_WITH_TBB
  Classification::classify<CGAL::Sequential_tag>
    (pts, labels, classifier, label_indices);

  Classification::classify_with_local_smoothing<CGAL::Sequential_tag>
    (pts, pts.point_map(), labels, classifier,
     generator.neighborhood().sphere_neighbor_query(0.01f),
     label_indices);

  Classification::classify_with_graphcut<CGAL::Sequential_tag>
    (pts, pts.point_map(), labels, classifier,
     generator.neighborhood().k_neighbor_query(12),
     0.2f, 10, label_indices);
#endif

  Classification::Evaluation evaluation (labels, training_set, label_indices);
  
  return EXIT_SUCCESS;
}