pcd_select_object_plane.cpp

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2012-, Open Perception, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder(s) nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: openni_viewer.cpp 5059 2012-03-14 02:12:17Z gedikli $
 *
 */

#include <pcl/common/angles.h>
#include <pcl/console/parse.h>
#include <pcl/console/print.h>
#include <pcl/console/time.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/geometry/polygon_operations.h>
#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/sac_model_plane.h> // for pointToPlaneDistance
#include <pcl/segmentation/edge_aware_plane_comparator.h>
#include <pcl/segmentation/euclidean_cluster_comparator.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <pcl/segmentation/organized_connected_component_segmentation.h>
#include <pcl/segmentation/organized_multi_plane_segmentation.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/visualization/image_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_handlers.h>

#include <thread>

using namespace pcl;
using namespace pcl::console;
using namespace std::chrono_literals;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
class ObjectSelection {
public:
  ObjectSelection()
  : plane_comparator_(new EdgeAwarePlaneComparator<PointT, Normal>), rgb_data_()
  {
    // Set the parameters for planar segmentation
    plane_comparator_->setDistanceThreshold(0.01f, false);
  }

  /////////////////////////////////////////////////////////////////////////
  virtual ~ObjectSelection() { delete[] rgb_data_; }

  /////////////////////////////////////////////////////////////////////////
  void
  estimateNormals(const typename PointCloud<PointT>::ConstPtr& input,
                  PointCloud<Normal>& normals)
  {
    if (input->isOrganized()) {
      IntegralImageNormalEstimation<PointT, Normal> ne;
      // Set the parameters for normal estimation
      ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
      ne.setMaxDepthChangeFactor(0.02f);
      ne.setNormalSmoothingSize(20.0f);
      // Estimate normals in the cloud
      ne.setInputCloud(input);
      ne.compute(normals);

      // Save the distance map for the plane comparator
      float* map = ne.getDistanceMap(); // This will be deallocated with the
                                        // IntegralImageNormalEstimation object...
      distance_map_.assign(map, map + input->size()); //...so we must copy the data out
      plane_comparator_->setDistanceMap(distance_map_.data());
    }
    else {
      NormalEstimation<PointT, Normal> ne;
      ne.setInputCloud(input);
      ne.setRadiusSearch(0.02f);
      ne.setSearchMethod(search_);
      ne.compute(normals);
    }
  }

  /////////////////////////////////////////////////////////////////////////
  void
  keyboard_callback(const visualization::KeyboardEvent&, void*)
  {
    // if (event.getKeyCode())
    //  std::cout << "the key \'" << event.getKeyCode() << "\' (" << event.getKeyCode()
    //  << ") was";
    // else
    //  std::cout << "the special key \'" << event.getKeySym() << "\' was";
    // if (event.keyDown())
    //  std::cout << " pressed" << std::endl;
    // else
    //  std::cout << " released" << std::endl;
  }

  /////////////////////////////////////////////////////////////////////////
  void
  mouse_callback(const visualization::MouseEvent& mouse_event, void*)
  {
    if (mouse_event.getType() == visualization::MouseEvent::MouseButtonPress &&
        mouse_event.getButton() == visualization::MouseEvent::LeftButton) {
      std::cout << "left button pressed @ " << mouse_event.getX() << " , "
                << mouse_event.getY() << std::endl;
    }
  }

  /////////////////////////////////////////////////////////////////////////
  /** \brief
   * Given a plane, and the set of inlier indices representing it,
   * segment out the object of intererest supported by it.
   *
   * \param[in] picked_idx the index of a point on the object
   * \param[in] cloud the full point cloud dataset
   * \param[in] plane_indices a set of indices representing the plane supporting the
   * object of interest
   * \param[out] object the segmented resultant object
   * \param[out] object the segmented resultant object
   */
  void
  segmentObject(pcl::index_t picked_idx,
                const typename PointCloud<PointT>::ConstPtr& cloud,
                const PointIndices::Ptr& plane_indices,
                PointCloud<PointT>& object)
  {
    typename PointCloud<PointT>::Ptr plane_hull(new PointCloud<PointT>);

    // Compute the convex hull of the plane
    ConvexHull<PointT> chull;
    chull.setDimension(2);
    chull.setInputCloud(cloud);
    chull.setIndices(plane_indices);
    chull.reconstruct(*plane_hull);

    // Remove the plane indices from the data
    typename PointCloud<PointT>::Ptr plane(new PointCloud<PointT>);
    ExtractIndices<PointT> extract(true);
    extract.setInputCloud(cloud);
    extract.setIndices(plane_indices);
    extract.setNegative(false);
    extract.filter(*plane);
    PointIndices::Ptr indices_but_the_plane(new PointIndices);
    extract.getRemovedIndices(*indices_but_the_plane);

    // Extract all clusters above the hull
    PointIndices::Ptr points_above_plane(new PointIndices);
    ExtractPolygonalPrismData<PointT> exppd;
    exppd.setInputCloud(cloud);
    exppd.setIndices(indices_but_the_plane);
    exppd.setInputPlanarHull(plane_hull);
    exppd.setViewPoint(
        (*cloud)[picked_idx].x, (*cloud)[picked_idx].y, (*cloud)[picked_idx].z);
    exppd.setHeightLimits(0.001, 0.5); // up to half a meter
    exppd.segment(*points_above_plane);

    std::vector<PointIndices> euclidean_label_indices;
    // Prefer a faster method if the cloud is organized, over EuclidanClusterExtraction
    if (cloud_->isOrganized()) {
      // Use an organized clustering segmentation to extract the individual clusters
      typename EuclideanClusterComparator<PointT, Label>::Ptr
          euclidean_cluster_comparator(new EuclideanClusterComparator<PointT, Label>);
      euclidean_cluster_comparator->setInputCloud(cloud);
      euclidean_cluster_comparator->setDistanceThreshold(0.03f, false);
      // Set the entire scene to false, and the inliers of the objects located on top of
      // the plane to true
      Label l;
      l.label = 0;
      PointCloud<Label>::Ptr scene(
          new PointCloud<Label>(cloud->width, cloud->height, l));
      // Mask the objects that we want to split into clusters
      for (const auto& index : points_above_plane->indices)
        (*scene)[index].label = 1;
      euclidean_cluster_comparator->setLabels(scene);

      typename EuclideanClusterComparator<PointT, Label>::ExcludeLabelSetPtr
          exclude_labels(
              new typename EuclideanClusterComparator<PointT, Label>::ExcludeLabelSet);
      exclude_labels->insert(0);
      euclidean_cluster_comparator->setExcludeLabels(exclude_labels);

      OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation(
          euclidean_cluster_comparator);
      euclidean_segmentation.setInputCloud(cloud);

      PointCloud<Label> euclidean_labels;
      euclidean_segmentation.segment(euclidean_labels, euclidean_label_indices);
    }
    else {
      print_highlight(
          stderr,
          "Extracting individual clusters from the points above the reference plane ");
      TicToc tt;
      tt.tic();

      EuclideanClusterExtraction<PointT> ec;
      ec.setClusterTolerance(0.02); // 2cm
      ec.setMinClusterSize(100);
      ec.setSearchMethod(search_);
      ec.setInputCloud(cloud);
      ec.setIndices(points_above_plane);
      ec.extract(euclidean_label_indices);

      print_info("[done, ");
      print_value("%g", tt.toc());
      print_info(" ms : ");
      print_value("%lu", euclidean_label_indices.size());
      print_info(" clusters]\n");
    }

    // For each cluster found
    bool cluster_found = false;
    for (const auto& euclidean_label_index : euclidean_label_indices) {
      if (cluster_found)
        break;
      // Check if the point that we picked belongs to it
      for (std::size_t j = 0; j < euclidean_label_index.indices.size(); ++j) {
        if (picked_idx != euclidean_label_index.indices[j])
          continue;
        copyPointCloud(*cloud, euclidean_label_index.indices, object);
        cluster_found = true;
        break;
      }
    }
  }

  /////////////////////////////////////////////////////////////////////////
  void
  segment(const PointT& picked_point,
          pcl::index_t picked_idx,
          PlanarRegion<PointT>& region,
          typename PointCloud<PointT>::Ptr& object)
  {
    object.reset();

    // Segment out all planes
    std::vector<ModelCoefficients> model_coefficients;
    std::vector<PointIndices> inlier_indices, boundary_indices;
    std::vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT>>>
        regions;

    // Prefer a faster method if the cloud is organized, over RANSAC
    if (cloud_->isOrganized()) {
      print_highlight(stderr, "Estimating normals ");
      TicToc tt;
      tt.tic();
      // Estimate normals
      PointCloud<Normal>::Ptr normal_cloud(new PointCloud<Normal>);
      estimateNormals(cloud_, *normal_cloud);
      print_info("[done, ");
      print_value("%g", tt.toc());
      print_info(" ms : ");
      print_value("%lu", normal_cloud->size());
      print_info(" points]\n");

      OrganizedMultiPlaneSegmentation<PointT, Normal, Label> mps;
      mps.setMinInliers(1000);
      mps.setAngularThreshold(deg2rad(3.0)); // 3 degrees
      mps.setDistanceThreshold(0.03);        // 2 cm
      mps.setMaximumCurvature(0.001);        // a small curvature
      mps.setProjectPoints(true);
      mps.setComparator(plane_comparator_);
      mps.setInputNormals(normal_cloud);
      mps.setInputCloud(cloud_);

      // Use one of the overloaded segmentAndRefine calls to get all the information
      // that we want out
      PointCloud<Label>::Ptr labels(new PointCloud<Label>);
      std::vector<PointIndices> label_indices;
      mps.segmentAndRefine(regions,
                           model_coefficients,
                           inlier_indices,
                           labels,
                           label_indices,
                           boundary_indices);
    }
    else {
      SACSegmentation<PointT> seg;
      seg.setOptimizeCoefficients(true);
      seg.setModelType(SACMODEL_PLANE);
      seg.setMethodType(SAC_RANSAC);
      seg.setMaxIterations(10000);
      seg.setDistanceThreshold(0.005);

      // Copy XYZ and Normals to a new cloud
      typename PointCloud<PointT>::Ptr cloud_segmented(new PointCloud<PointT>(*cloud_));
      typename PointCloud<PointT>::Ptr cloud_remaining(new PointCloud<PointT>);

      ModelCoefficients coefficients;
      ExtractIndices<PointT> extract;
      PointIndices::Ptr inliers(new PointIndices());

      // Up until 30% of the original cloud is left
      int i = 1;
      while (double(cloud_segmented->size()) > 0.3 * double(cloud_->size())) {
        seg.setInputCloud(cloud_segmented);

        print_highlight(stderr, "Searching for the largest plane (%2.0d) ", i++);
        TicToc tt;
        tt.tic();
        seg.segment(*inliers, coefficients);
        print_info("[done, ");
        print_value("%g", tt.toc());
        print_info(" ms : ");
        print_value("%lu", inliers->indices.size());
        print_info(" points]\n");

        // No datasets could be found anymore
        if (inliers->indices.empty())
          break;

        // Save this plane
        PlanarRegion<PointT> region;
        region.setCoefficients(coefficients);
        regions.push_back(region);

        inlier_indices.push_back(*inliers);
        model_coefficients.push_back(coefficients);

        // Extract the outliers
        extract.setInputCloud(cloud_segmented);
        extract.setIndices(inliers);
        extract.setNegative(true);
        extract.filter(*cloud_remaining);
        cloud_segmented.swap(cloud_remaining);
      }
    }
    print_highlight(
        "Number of planar regions detected: %lu for a cloud of %lu points\n",
        regions.size(),
        cloud_->size());

    double max_dist = std::numeric_limits<double>::max();
    // Compute the distances from all the planar regions to the picked point, and select
    // the closest region
    int idx = -1;
    for (std::size_t i = 0; i < regions.size(); ++i) {
      double dist = pointToPlaneDistance(picked_point, regions[i].getCoefficients());
      if (dist < max_dist) {
        max_dist = dist;
        idx = static_cast<int>(i);
      }
    }

    // Get the plane that holds the object of interest
    if (idx != -1) {
      plane_indices_.reset(new PointIndices(inlier_indices[idx]));

      if (cloud_->isOrganized()) {
        approximatePolygon(regions[idx], region, 0.01f, false, true);
        print_highlight(
            "Planar region: %lu points initial, %lu points after refinement.\n",
            regions[idx].getContour().size(),
            region.getContour().size());
      }
      else {
        // Save the current region
        region = regions[idx];

        print_highlight(stderr, "Obtaining the boundary points for the region ");
        TicToc tt;
        tt.tic();
        // Project the inliers to obtain a better hull
        typename PointCloud<PointT>::Ptr cloud_projected(new PointCloud<PointT>);
        ModelCoefficients::Ptr coefficients(
            new ModelCoefficients(model_coefficients[idx]));
        ProjectInliers<PointT> proj;
        proj.setModelType(SACMODEL_PLANE);
        proj.setInputCloud(cloud_);
        proj.setIndices(plane_indices_);
        proj.setModelCoefficients(coefficients);
        proj.filter(*cloud_projected);

        // Compute the boundary points as a ConvexHull
        ConvexHull<PointT> chull;
        chull.setDimension(2);
        chull.setInputCloud(cloud_projected);
        PointCloud<PointT> plane_hull;
        chull.reconstruct(plane_hull);
        region.setContour(plane_hull);
        print_info("[done, ");
        print_value("%g", tt.toc());
        print_info(" ms : ");
        print_value("%lu", plane_hull.size());
        print_info(" points]\n");
      }
    }

    // Segment the object of interest
    if (plane_indices_ && !plane_indices_->indices.empty()) {
      plane_.reset(new PointCloud<PointT>);
      copyPointCloud(*cloud_, plane_indices_->indices, *plane_);
      object.reset(new PointCloud<PointT>);
      segmentObject(picked_idx, cloud_, plane_indices_, *object);
    }
  }

  /////////////////////////////////////////////////////////////////////////
  /**
   * \brief Point picking callback. This gets called when the user selects
   * a 3D point on screen (in the PCLVisualizer window) using Shift+click.
   *
   * \param[in] event the event that triggered the call
   */
  void
  pp_callback(const visualization::PointPickingEvent& event, void*)
  {
    // Check to see if we got a valid point. Early exit.
    int idx = event.getPointIndex();
    if (idx == -1)
      return;

    pcl::Indices indices(1);
    std::vector<float> distances(1);

    // Get the point that was picked
    PointT picked_pt;
    event.getPoint(picked_pt.x, picked_pt.y, picked_pt.z);

    print_info(stderr,
               "Picked point with index %d, and coordinates %f, %f, %f.\n",
               idx,
               picked_pt.x,
               picked_pt.y,
               picked_pt.z);

    // Add a sphere to it in the PCLVisualizer window
    const std::string sphere_name = "sphere_" + std::to_string(idx);
    cloud_viewer_->addSphere(picked_pt, 0.01, 1.0, 0.0, 0.0, sphere_name);

    // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we
    // must search for the real point
    search_->nearestKSearch(picked_pt, 1, indices, distances);

    // Add some marker to the image
    if (image_viewer_) {
      // Get the [u, v] in pixel coordinates for the ImageViewer. Remember that 0,0 is
      // bottom left.
      std::uint32_t width = search_->getInputCloud()->width,
                    height = search_->getInputCloud()->height;
      int v = height - indices[0] / width, u = indices[0] % width;

      image_viewer_->addCircle(u, v, 5, 1.0, 0.0, 0.0, "circles", 1.0);
      image_viewer_->addFilledRectangle(
          u - 5, u + 5, v - 5, v + 5, 0.0, 1.0, 0.0, "boxes", 0.5);
      image_viewer_->markPoint(
          u, v, visualization::red_color, visualization::blue_color, 10);
    }

    // Segment the region that we're interested in, by employing a two step process:
    //  * first, segment all the planes in the scene, and find the one closest to our
    //  picked point
    //  * then, use euclidean clustering to find the object that we clicked on and
    //  return it
    PlanarRegion<PointT> region;
    segment(picked_pt, indices[0], region, object_);

    // If no region could be determined, exit
    if (region.getContour().empty()) {
      PCL_ERROR("No planar region detected. Please select another point or relax the "
                "thresholds and continue.\n");
      return;
    }
    // Else, draw it on screen
    cloud_viewer_->addPolygon(region, 0.0, 0.0, 1.0, "region");
    cloud_viewer_->setShapeRenderingProperties(
        visualization::PCL_VISUALIZER_LINE_WIDTH, 10, "region");

    // Draw in image space
    if (image_viewer_) {
      image_viewer_->addPlanarPolygon(
          search_->getInputCloud(), region, 0.0, 0.0, 1.0, "refined_region", 1.0);
    }

    // If no object could be determined, exit
    if (!object_) {
      PCL_ERROR("No object detected. Please select another point or relax the "
                "thresholds and continue.\n");
      return;
    }
    // Visualize the object in 3D...
    visualization::PointCloudColorHandlerCustom<PointT> red(object_, 255, 0, 0);
    if (!cloud_viewer_->updatePointCloud(object_, red, "object"))
      cloud_viewer_->addPointCloud(object_, red, "object");
    // ...and 2D
    if (image_viewer_) {
      image_viewer_->removeLayer("object");
      image_viewer_->addMask(search_->getInputCloud(), *object_, "object");
    }

    // ...and 2D
    if (image_viewer_)
      image_viewer_->addRectangle(search_->getInputCloud(), *object_);
  }

  /////////////////////////////////////////////////////////////////////////
  void
  compute()
  {
    // Visualize the data
    while (!cloud_viewer_->wasStopped()) {
      /*// Add the plane that we're tracking to the cloud visualizer
      PointCloud<PointT>::Ptr plane (new Cloud);
      if (plane_)
        *plane = *plane_;
      visualization::PointCloudColorHandlerCustom<PointT> blue (plane, 0, 255, 0);
      if (!cloud_viewer_->updatePointCloud (plane, blue, "plane"))
        cloud_viewer_->addPointCloud (plane, "plane");
*/
      cloud_viewer_->spinOnce();
      if (image_viewer_) {
        image_viewer_->spinOnce();
        if (image_viewer_->wasStopped())
          break;
      }
      std::this_thread::sleep_for(100us);
    }
  }

  /////////////////////////////////////////////////////////////////////////
  void
  initGUI()
  {
    cloud_viewer_.reset(new visualization::PCLVisualizer("PointCloud"));

    if (cloud_->isOrganized()) {
      // If the dataset is organized, and has RGB data, create an image viewer
      std::vector<pcl::PCLPointField> fields;
      int rgba_index = -1;
      rgba_index = getFieldIndex<PointT>("rgba", fields);

      if (rgba_index >= 0) {
        image_viewer_.reset(new visualization::ImageViewer("RGB PCLImage"));

        image_viewer_->registerMouseCallback(&ObjectSelection::mouse_callback, *this);
        image_viewer_->registerKeyboardCallback(&ObjectSelection::keyboard_callback,
                                                *this);
        image_viewer_->setPosition(cloud_->width, 0);
        image_viewer_->setSize(cloud_->width, cloud_->height);

        int poff = fields[rgba_index].offset;
        // BGR to RGB
        rgb_data_ = new unsigned char[cloud_->width * cloud_->height * 3];
        for (std::uint32_t i = 0; i < cloud_->width * cloud_->height; ++i) {
          RGB rgb;
          memcpy(&rgb,
                 reinterpret_cast<unsigned char*>(&(*cloud_)[i]) + poff,
                 sizeof(rgb));

          rgb_data_[i * 3 + 0] = rgb.r;
          rgb_data_[i * 3 + 1] = rgb.g;
          rgb_data_[i * 3 + 2] = rgb.b;
        }
        image_viewer_->showRGBImage(rgb_data_, cloud_->width, cloud_->height);
      }
      cloud_viewer_->setSize(cloud_->width, cloud_->height);
    }

    cloud_viewer_->registerMouseCallback(&ObjectSelection::mouse_callback, *this);
    cloud_viewer_->registerKeyboardCallback(&ObjectSelection::keyboard_callback, *this);
    cloud_viewer_->registerPointPickingCallback(&ObjectSelection::pp_callback, *this);
    cloud_viewer_->setPosition(0, 0);

    cloud_viewer_->addPointCloud(cloud_, "scene");
    cloud_viewer_->resetCameraViewpoint("scene");
    cloud_viewer_->addCoordinateSystem(0.1, 0, 0, 0, "global");
  }

  /////////////////////////////////////////////////////////////////////////
  bool
  load(const std::string& file)
  {
    // Load the input file
    TicToc tt;
    tt.tic();
    print_highlight(stderr, "Loading ");
    print_value(stderr, "%s ", file.c_str());
    cloud_.reset(new PointCloud<PointT>);
    if (io::loadPCDFile(file, *cloud_) < 0) {
      print_error(stderr, "[error]\n");
      return false;
    }
    print_info("[done, ");
    print_value("%g", tt.toc());
    print_info(" ms : ");
    print_value("%lu", cloud_->size());
    print_info(" points]\n");

    if (cloud_->isOrganized())
      search_.reset(new search::OrganizedNeighbor<PointT>);
    else
      search_.reset(new search::KdTree<PointT>);

    search_->setInputCloud(cloud_);

    return true;
  }

  /////////////////////////////////////////////////////////////////////////
  void
  save(const std::string& object_file, const std::string& plane_file)
  {
    PCDWriter w;
    if (object_ && !object_->empty()) {
      w.writeBinaryCompressed(object_file, *object_);
      w.writeBinaryCompressed(plane_file, *plane_);
      print_highlight("Object successfully segmented. Saving results in: %s, and %s.\n",
                      object_file.c_str(),
                      plane_file.c_str());
    }
  }

  visualization::PCLVisualizer::Ptr cloud_viewer_;
  visualization::ImageViewer::Ptr image_viewer_;

  typename PointCloud<PointT>::Ptr cloud_;
  typename search::Search<PointT>::Ptr search_;

private:
  // Segmentation
  typename EdgeAwarePlaneComparator<PointT, Normal>::Ptr plane_comparator_;
  PointIndices::Ptr plane_indices_;
  unsigned char* rgb_data_;
  std::vector<float> distance_map_;

  // Results
  typename PointCloud<PointT>::Ptr plane_;
  typename PointCloud<PointT>::Ptr object_;
};

/* ---[ */
int
main(int argc, char** argv)
{
  // Parse the command line arguments for .pcd files
  std::vector<int> p_file_indices;
  p_file_indices = parse_file_extension_argument(argc, argv, ".pcd");
  if (p_file_indices.empty()) {
    print_error("  Need at least an input PCD file (e.g. scene.pcd) to continue!\n\n");
    print_info("Ideally, need an input file, and three output PCD files, e.g., "
               "object.pcd, plane.pcd, rest.pcd\n");
    return -1;
  }

  std::string object_file = "object.pcd";
  std::string plane_file = "plane.pcd";
  if (p_file_indices.size() >= 3)
    plane_file = argv[p_file_indices[2]];
  if (p_file_indices.size() >= 2)
    object_file = argv[p_file_indices[1]];

  PCDReader reader;
  // Test the header
  pcl::PCLPointCloud2 dummy;
  reader.readHeader(argv[p_file_indices[0]], dummy);
  if (dummy.height != 1 && getFieldIndex(dummy, "rgba") != -1) {
    print_highlight("Enabling 2D image viewer mode.\n");
    ObjectSelection<PointXYZRGBA> s;
    if (!s.load(argv[p_file_indices[0]]))
      return -1;
    s.initGUI();
    s.compute();
    s.save(object_file, plane_file);
  }
  else {
    ObjectSelection<PointXYZ> s;
    if (!s.load(argv[p_file_indices[0]]))
      return -1;
    s.initGUI();
    s.compute();
    s.save(object_file, plane_file);
  }

  return 0;
}
/* ]--- */