void
OrganizedMultiPlaneExtractor<PointT>::compute()
{
    CHECK ( cloud_ && cloud_->isOrganized() ) << "Input cloud is not organized!";
    CHECK ( normal_cloud_ && (normal_cloud_->points.size() == cloud_->points.size() ));

    pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
    mps.setMinInliers ( param_.min_num_plane_inliers_ );
    mps.setAngularThreshold ( param_.angular_threshold_deg_ * M_PI/180.f );
    mps.setDistanceThreshold ( param_.distance_threshold_);
    mps.setInputNormals ( normal_cloud_ );
    mps.setInputCloud ( cloud_ );

    std::vector < pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
    typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr ref_comp (
                new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ());
    ref_comp->setDistanceThreshold (param_.distance_threshold_, false );
    ref_comp->setAngularThreshold ( param_.angular_threshold_deg_ * M_PI/180.f );
    mps.setRefinementComparator ( ref_comp );
    mps.segmentAndRefine ( regions );

    all_planes_.clear();
    all_planes_.reserve (regions.size ());
    for (const pcl::PlanarRegion<PointT> &reg : regions )
    {
        Eigen::Vector4f plane = reg.getCoefficients();

        // flip plane normal towards viewpoint
        Eigen::Vector3f z = Eigen::Vector3f::UnitZ();
        if( z.dot(plane.head(3)) > 0 )
            plane = -plane;

      all_planes_.push_back( plane );
    }
}
Ejemplo n.º 2
0
void
MultiplaneSegmenter<PointT>::computeTablePlanes()
{
    all_planes_.clear();
    pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
    mps.setMinInliers (param_.num_plane_inliers_);
    mps.setAngularThreshold ( pcl::deg2rad(param_.angular_threshold_deg_) );
    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 ( pcl::deg2rad(2.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;

    all_planes_.resize ( model_coeff.size() );
    for (size_t i = 0; i < model_coeff.size (); i++)
    {
        // flip normal of plane towards viewpoint
        Eigen::Vector3f vp;
        vp(0)=vp(1)=0.f; vp(2) = 1;

        Eigen::Vector4f plane_tmp = Eigen::Vector4f(model_coeff[i].values[0], model_coeff[i].values[1], model_coeff[i].values[2], model_coeff[i].values[3]);
        Eigen::Vector3f table_vec = plane_tmp.head(3);
        if(vp.dot(table_vec)>0)
            plane_tmp *= -1.f;

        all_planes_[i].reset( new PlaneModel<PointT>);
        all_planes_[i]->coefficients_ = plane_tmp;
        all_planes_[i]->inliers_ = inlier_indices[i].indices;
        all_planes_[i]->cloud_ = scene_;


        typename pcl::PointCloud<PointT>::Ptr plane_cloud (new pcl::PointCloud<PointT>);
        typename pcl::PointCloud<PointT>::Ptr above_plane_cloud (new pcl::PointCloud<PointT>);
        pcl::copyPointCloud(*scene_, inlier_indices[i], *plane_cloud);

        double z_min = 0., z_max = 0.30; // we want the points above the plane, no farther than zmax cm from the surface
        typename pcl::PointCloud<PointT>::Ptr hull_points = all_planes_[i]->getConvexHullCloud();

        pcl::PointIndices cloud_indices;
        pcl::ExtractPolygonalPrismData<PointT> prism;
        prism.setInputCloud (scene_);
        prism.setInputPlanarHull (hull_points);
        prism.setHeightLimits (z_min, z_max);
        prism.segment (cloud_indices);

        pcl::copyPointCloud(*scene_, cloud_indices, *above_plane_cloud);

        pcl::visualization::PCLVisualizer::Ptr vis;
        int vp1, vp2;
        if(!vis)
        {
            vis.reset (new pcl::visualization::PCLVisualizer("plane22 visualization"));
            vis->createViewPort(0,0,0.5,1,vp1);
            vis->createViewPort(0.5,0,1,1,vp2);
        }
        vis->removeAllPointClouds();
        vis->removeAllShapes();
        vis->addPointCloud(scene_, "cloud", vp1);

        vis->addPointCloud(above_plane_cloud, "convex_hull", vp2);
        vis->spin();

        all_planes_[i]->visualize();
    }
}
    void
    process ()
    {
      std::cout << "threshold: " << threshold_ << std::endl;
      std::cout << "depth dependent: " << (depth_dependent_ ? "true\n" : "false\n");
      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::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
      ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
      ne.setMaxDepthChangeFactor (0.02f);
      ne.setNormalSmoothingSize (20.0f);
      
      typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr refinement_compare (new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ());
      refinement_compare->setDistanceThreshold (threshold_, depth_dependent_);
      
      pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
      mps.setMinInliers (5000);
      mps.setAngularThreshold (0.017453 * 3.0); //3 degrees
      mps.setDistanceThreshold (0.03); //2cm
      mps.setRefinementComparator (refinement_compare);
      
      std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
      typename pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>);
      typename pcl::PointCloud<PointT>::Ptr approx_contour (new pcl::PointCloud<PointT>);
      char name[1024];

      typename pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
      double normal_start = pcl::getTime ();
      ne.setInputCloud (cloud);
      ne.compute (*normal_cloud);
      double normal_end = pcl::getTime ();
      std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl;

      double plane_extract_start = pcl::getTime ();
      mps.setInputNormals (normal_cloud);
      mps.setInputCloud (cloud);
      if (refine_)
        mps.segmentAndRefine (regions);
      else
        mps.segment (regions);
      double plane_extract_end = pcl::getTime ();
      std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << " with planar regions found: " << regions.size () << std::endl;
      std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl;

      typename pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);

      viewer.removeAllPointClouds (0);
      viewer.removeAllShapes (0);
      pcl::visualization::PointCloudColorHandlerCustom<PointT> single_color (cloud, 0, 255, 0);
      viewer.addPointCloud<PointT> (cloud, single_color, "cloud");
      
      pcl::PlanarPolygon<PointT> approx_polygon;
      //Draw Visualization
      for (size_t i = 0; i < regions.size (); i++)
      {
        Eigen::Vector3f centroid = regions[i].getCentroid ();
        Eigen::Vector4f model = regions[i].getCoefficients ();
        pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]);
        pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]),
                                           centroid[1] + (0.5f * model[1]),
                                           centroid[2] + (0.5f * model[2]));
        sprintf (name, "normal_%d", unsigned (i));
        viewer.addArrow (pt2, pt1, 1.0, 0, 0, std::string (name));

        contour->points = regions[i].getContour ();        
        sprintf (name, "plane_%02d", int (i));
        pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]);
        viewer.addPointCloud (contour, color, name);

        pcl::approximatePolygon (regions[i], approx_polygon, threshold_, polygon_refinement_);
        approx_contour->points = approx_polygon.getContour ();
        std::cout << "polygon: " << contour->size () << " -> " << approx_contour->size () << std::endl;
        typename pcl::PointCloud<PointT>::ConstPtr approx_contour_const = approx_contour;
        
//        sprintf (name, "approx_plane_%02d", int (i));
//        viewer.addPolygon<PointT> (approx_contour_const, 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name);
        for (unsigned idx = 0; idx < approx_contour->points.size (); ++idx)
        {
          sprintf (name, "approx_plane_%02d_%03d", int (i), int(idx));
          viewer.addLine (approx_contour->points [idx], approx_contour->points [(idx+1)%approx_contour->points.size ()], 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name);
        }
      }
    }
void
v4r::MultiPlaneSegmentation<PointT>::segment(bool force_unorganized)
{
  models_.clear();

  if(input_->isOrganized() && !force_unorganized)
  {
    pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
    if(!normals_set_)
    {
        pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
        ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
        ne.setMaxDepthChangeFactor (0.02f);
        ne.setNormalSmoothingSize (20.0f);
        ne.setBorderPolicy (pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::BORDER_POLICY_IGNORE);
        ne.setInputCloud (input_);
        ne.compute (*normal_cloud);
    }
    else
    {
        normal_cloud = normal_cloud_;
    }

    pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
    mps.setMinInliers (min_plane_inliers_);
    mps.setAngularThreshold (0.017453 * 2.f); // 2 degrees
    mps.setDistanceThreshold (0.01); // 1cm
    mps.setMaximumCurvature(0.002);
    mps.setInputNormals (normal_cloud);
    mps.setInputCloud (input_);

    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;

    typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr ref_comp (
                                                                                             new pcl::PlaneRefinementComparator<PointT,
                                                                                                 pcl::Normal, pcl::Label> ());
    ref_comp->setDistanceThreshold (0.01f, false);
    ref_comp->setAngularThreshold (0.017453 * 2.f);
    mps.setRefinementComparator (ref_comp);
    mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
    //mps.segment (model_coefficients, inlier_indices);

    //std::cout << model_coefficients.size() << std::endl;

    if(merge_planes_)
    {
      //sort planes by size
      //check if the first plane can be merged against the others, if yes, define a new plane combining both and add it to the cue

      typedef boost::adjacency_matrix<boost::undirectedS, int> GraphPlane;
      GraphPlane mergeable_planes (model_coefficients.size ());
      for(size_t i=0; i < model_coefficients.size(); i++)
      {

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

        plane_i.normalize();
        for(size_t j=(i+1); j < model_coefficients.size(); j++)
        {
          Eigen::Vector3f plane_j = Eigen::Vector3f (model_coefficients[j].values[0], model_coefficients[j].values[1],
                                                     model_coefficients[j].values[2]);

          plane_j.normalize();

          //std::cout << "dot product:" << plane_i.dot(plane_j) << " diff:" << std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) << std::endl;
          if(plane_i.dot(plane_j) > 0.95)
          {
            if(std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) < 0.015)
            {
              boost::add_edge (static_cast<int>(i), static_cast<int>(j), mergeable_planes);
            }
          }
        }
      }

      boost::vector_property_map<int> components (boost::num_vertices (mergeable_planes));
      int n_cc = static_cast<int> (boost::connected_components (mergeable_planes, &components[0]));

      std::vector<int> cc_sizes;
      std::vector< std::vector<int> > cc_to_model_coeff;
      cc_sizes.resize (n_cc, 0);
      cc_to_model_coeff.resize(n_cc);

      for (size_t i = 0; i < model_coefficients.size (); i++)
      {
        cc_sizes[components[i]]++;
        cc_to_model_coeff[components[i]].push_back(i);
      }

      std::vector<pcl::ModelCoefficients> new_model_coefficients;
      std::vector<pcl::PointIndices> new_inlier_indices;

      for(size_t i=0; i < cc_sizes.size(); i++)
      {
        if(cc_sizes[i] < 2)
        {
          new_model_coefficients.push_back(model_coefficients[cc_to_model_coeff[i][0]]);
          new_inlier_indices.push_back(inlier_indices[cc_to_model_coeff[i][0]]);
          continue;
        }

        //std::cout << "going to merge CC:" << cc_sizes[i] << std::endl;
        pcl::ModelCoefficients model_coeff;
        model_coeff.values.resize(4);

        for(size_t k=0; k < 4; k++)
          model_coeff.values[k] = 0.f;

        pcl::PointIndices merged_indices;
        for(size_t j=0; j < cc_to_model_coeff[i].size(); j++)
        {
          for(size_t k=0; k < 4; k++)
            model_coeff.values[k] += model_coefficients[cc_to_model_coeff[i][j]].values[k];

          merged_indices.indices.insert(merged_indices.indices.end(), inlier_indices[cc_to_model_coeff[i][j]].indices.begin(),
                                                                      inlier_indices[cc_to_model_coeff[i][j]].indices.end());
        }

        for(size_t k=0; k < 4; k++)
          model_coeff.values[k] /= static_cast<float>(cc_to_model_coeff[i].size());

        new_model_coefficients.push_back(model_coeff);
        new_inlier_indices.push_back(merged_indices);
      }

      model_coefficients = new_model_coefficients;
      inlier_indices = new_inlier_indices;
    }

    for(size_t i=0; i < model_coefficients.size(); i++)
    {
      PlaneModel<PointT> pm;
      pm.coefficients_ = model_coefficients[i];
      pm.cloud_ = input_;
      pm.inliers_ = inlier_indices[i];

      //recompute coefficients based on distance to camera and normal?
      Eigen::Vector4f centroid;
      pcl::compute3DCentroid(*pm.cloud_, pm.inliers_, centroid);
      Eigen::Vector3f c(centroid[0],centroid[1],centroid[2]);

      Eigen::MatrixXf M_w(inlier_indices[i].indices.size(), 3);

      float sum_w = 0.f;
      for(size_t k=0; k < inlier_indices[i].indices.size(); k++)
      {
          const int &idx = inlier_indices[i].indices[k];
          float d_c = (pm.cloud_->points[idx].getVector3fMap()).norm();
          float w_k = std::max(1.f - std::abs(1.f - d_c), 0.f);
          //w_k = 1.f;
          M_w.row(k) = w_k * (pm.cloud_->points[idx].getVector3fMap() - c);
          sum_w += w_k;
      }

      Eigen::Matrix3f scatter;
      scatter.setZero ();
      scatter = M_w.transpose() * M_w;

      Eigen::JacobiSVD<Eigen::MatrixXf> svd(scatter, Eigen::ComputeFullV);
      //std::cout << svd.matrixV() << std::endl;

      Eigen::Vector3f n = svd.matrixV().col(2);
      //flip normal if required
      if(n.dot(c*-1) < 0)
          n = n * -1.f;

      float d = n.dot(c) * -1.f;
      //std::cout << "normal:" << n << std::endl;
      //std::cout << "d:" << d << std::endl;

      pm.coefficients_.values[0] = n[0];
      pm.coefficients_.values[1] = n[1];
      pm.coefficients_.values[2] = n[2];
      pm.coefficients_.values[3] = d;

      pcl::PointIndices clean_inlier_indices;
      float dist_threshold_ = 0.01f;

      for(size_t k=0; k < inlier_indices[i].indices.size(); k++)
      {
          const int &idx = inlier_indices[i].indices[k];
          Eigen::Vector3f p = pm.cloud_->points[idx].getVector3fMap();
          float val = n.dot(p) + d;

          if(std::abs(val) <= dist_threshold_)
              clean_inlier_indices.indices.push_back( idx );
      }

      pm.inliers_ = clean_inlier_indices;
      models_.push_back(pm);
    }
  }
  else
  {
    // Create the filtering object: downsample the dataset using a leaf size of 1cm
    pcl::VoxelGrid<PointT> vg;
    PointTCloudPtr cloud_filtered (new PointTCloud);
    vg.setInputCloud (input_);
    float leaf_size_ = 0.005f;
    float dist_threshold_ = 0.01f;
    vg.setLeafSize (leaf_size_, leaf_size_, leaf_size_);
    vg.filter (*cloud_filtered);

    std::vector<bool> pixel_has_not_been_labelled( cloud_filtered->points.size(), true );

    // Create the segmentation object for the planar model and set all the parameters
    pcl::SACSegmentation<PointT> seg;
    pcl::ModelCoefficients coefficients;
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setMaxIterations (1000);
    seg.setDistanceThreshold (dist_threshold_);

    PointTCloudPtr cloud_filtered_leftover (new PointTCloud (*cloud_filtered));

    while (1)
    {
      // Segment the largest planar component from the remaining cloud
      seg.setInputCloud (cloud_filtered_leftover);
      pcl::PointIndices inliers_in_leftover;
      seg.segment (inliers_in_leftover, coefficients);

      std::cout << "inliers in left over: " << inliers_in_leftover.indices.size() << " of " << cloud_filtered_leftover->points.size() << std::endl;

      if ( (int)inliers_in_leftover.indices.size () < min_plane_inliers_) // Could not estimate a(nother) planar model big enough for the given cloud.
        break;


      // make indices correspond to complete downsampled cloud
      pcl::PointIndices indices_in_original_cloud;
      size_t current_index_in_leftover = 0;
      size_t px=0;
      indices_in_original_cloud.indices.resize(inliers_in_leftover.indices.size());
      for(size_t inl_id=0; inl_id < inliers_in_leftover.indices.size(); inl_id++) {    // assumes indices are sorted in ascending order
          bool found = false;
          do {
              if( pixel_has_not_been_labelled[px] ) {
                  if( current_index_in_leftover == inliers_in_leftover.indices [inl_id] ) {
                      indices_in_original_cloud.indices[ inl_id ] = px;
                      found = true;
                  }
                  current_index_in_leftover++;
              }
              px++;
          } while( !found );
      }

      for(size_t i=0; i<indices_in_original_cloud.indices.size(); i++)
          pixel_has_not_been_labelled[ indices_in_original_cloud.indices[i] ] = false;


      //save coefficients
        PlaneModel<PointT> pm;
        pm.coefficients_ = coefficients;
        pm.cloud_ = cloud_filtered;
        pm.inliers_ = indices_in_original_cloud;
        models_.push_back(pm);

        pcl::copyPointCloud(*cloud_filtered, pixel_has_not_been_labelled, *cloud_filtered_leftover);
    }

    std::cout << "Number of planes found:" << models_.size() << "organized:" << static_cast<int>(input_->isOrganized() && !force_unorganized) << std::endl;
  }
}
Ejemplo n.º 5
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]);
        }
    }
}