Example #1
0
void Clipper::splitClip() {
	if (clipMode() && valid()) {
		Vector3 planepts[3];
		AABB bounds(Vector3(0, 0, 0), Vector3(64, 64, 64));
		getPlanePoints(planepts, bounds);

		splitBrushes(planepts[0], planepts[1], planepts[2], eFrontAndBack);

		reset();
		update();
	}
}
Example #2
0
void Clipper::update() {
	Vector3 planepts[3];
	if (!valid()) {
		planepts[0] = Vector3(0, 0, 0);
		planepts[1] = Vector3(0, 0, 0);
		planepts[2] = Vector3(0, 0, 0);
		setClipPlane(Plane3(0, 0, 0, 0));
	}
	else {
		AABB bounds(Vector3(0, 0, 0), Vector3(64, 64, 64));
		getPlanePoints(planepts, bounds);
		if (_switch) {
			std::swap(planepts[0], planepts[1]);
		}
		setClipPlane(Plane3(planepts));
	}
	GlobalMainFrame().updateAllWindows();
}
bool TableDetector::detectTable(const sensor_msgs::PointCloud2 &cloud, tabletop_object_detector::Table& table)
{
  ROS_INFO("Starting process on new cloud");
  ROS_INFO("In frame %s", cloud.header.frame_id.c_str());

  // PCL objects
  KdTreePtr normals_tree_, clusters_tree_;
  pcl::VoxelGrid<Point> grid_, grid_objects_;
  pcl::PassThrough<Point> pass_;
  pcl::NormalEstimation<Point, pcl::Normal> n3d_;
  pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_;
  pcl::ProjectInliers<Point> proj_;
  pcl::ConvexHull<Point> hull_;
  pcl::ExtractPolygonalPrismData<Point> prism_;
  pcl::EuclideanClusterExtraction<Point> pcl_cluster_;

  // Filtering parameters
  grid_.setLeafSize (plane_detection_voxel_size_, plane_detection_voxel_size_, plane_detection_voxel_size_);
  grid_objects_.setLeafSize (clustering_voxel_size_, clustering_voxel_size_, clustering_voxel_size_);
  grid_.setFilterFieldName ("z");
  pass_.setFilterFieldName ("z");

  pass_.setFilterLimits (z_filter_min_, z_filter_max_);
  grid_.setFilterLimits (z_filter_min_, z_filter_max_);
  grid_.setDownsampleAllData (false);
  grid_objects_.setDownsampleAllData (false);

  normals_tree_ = boost::make_shared<pcl::KdTreeFLANN<Point> > ();
  clusters_tree_ = boost::make_shared<pcl::KdTreeFLANN<Point> > ();

  // Normal estimation parameters
  n3d_.setKSearch (10);
  n3d_.setSearchMethod (normals_tree_);
  // Table model fitting parameters
  seg_.setDistanceThreshold (0.05);
  seg_.setMaxIterations (10000);
  seg_.setNormalDistanceWeight (0.1);
  seg_.setOptimizeCoefficients (true);
  seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
  seg_.setMethodType (pcl::SAC_RANSAC);
  seg_.setProbability (0.99);

  proj_.setModelType (pcl::SACMODEL_PLANE);

  // Consider only objects in a given layer above the table
  prism_.setHeightLimits (table_z_filter_min_, table_z_filter_max_);

  // Clustering parameters
  pcl_cluster_.setClusterTolerance (cluster_distance_);
  pcl_cluster_.setMinClusterSize (min_cluster_size_);
  pcl_cluster_.setSearchMethod (clusters_tree_);

  // Step 1 : Filter, remove NaNs and downsample
  pcl::PointCloud<Point> cloud_t;
  pcl::fromROSMsg (cloud, cloud_t);
  pcl::PointCloud<Point>::ConstPtr cloud_ptr = boost::make_shared<const pcl::PointCloud<Point> > (cloud_t);

  pcl::PointCloud<Point> cloud_filtered;
  pass_.setInputCloud (cloud_ptr);
  pass_.filter (cloud_filtered);
  pcl::PointCloud<Point>::ConstPtr cloud_filtered_ptr =
    boost::make_shared<const pcl::PointCloud<Point> > (cloud_filtered);
  ROS_INFO("Step 1 done");
  if (cloud_filtered.points.size() < (unsigned int)min_cluster_size_)
  {
    ROS_INFO("Filtered cloud only has %d points", (int)cloud_filtered.points.size());
    return false;
  }

  pcl::PointCloud<Point> cloud_downsampled;
  grid_.setInputCloud (cloud_filtered_ptr);
  grid_.filter (cloud_downsampled);
  pcl::PointCloud<Point>::ConstPtr cloud_downsampled_ptr =
    boost::make_shared<const pcl::PointCloud<Point> > (cloud_downsampled);
  if (cloud_downsampled.points.size() < (unsigned int)min_cluster_size_)
  {
    ROS_INFO("Downsampled cloud only has %d points", (int)cloud_downsampled.points.size());
    return false;
  }

  // Step 2 : Estimate normals
  pcl::PointCloud<pcl::Normal> cloud_normals;
  n3d_.setInputCloud (cloud_downsampled_ptr);
  n3d_.compute (cloud_normals);
  pcl::PointCloud<pcl::Normal>::ConstPtr cloud_normals_ptr =
    boost::make_shared<const pcl::PointCloud<pcl::Normal> > (cloud_normals);
  ROS_INFO("Step 2 done");

  // Step 3 : Perform planar segmentation
  pcl::PointIndices table_inliers; pcl::ModelCoefficients table_coefficients;
  seg_.setInputCloud (cloud_downsampled_ptr);
  seg_.setInputNormals (cloud_normals_ptr);
  seg_.segment (table_inliers, table_coefficients);
  pcl::PointIndices::ConstPtr table_inliers_ptr = boost::make_shared<const pcl::PointIndices> (table_inliers);
  pcl::ModelCoefficients::ConstPtr table_coefficients_ptr =
    boost::make_shared<const pcl::ModelCoefficients> (table_coefficients);

  if (table_coefficients.values.size () <=3)
  {
    ROS_INFO("Failed to detect table in scan");
    return false;
  }

  if ( table_inliers.indices.size() < (unsigned int)inlier_threshold_)
  {
    ROS_INFO("Plane detection has %d inliers, below min threshold of %d", (int)table_inliers.indices.size(),
       inlier_threshold_);
    return false;
  }

  ROS_INFO ("[TableObjectDetector::input_callback] Model found with %d inliers: [%f %f %f %f].",
      (int)table_inliers.indices.size (),
      table_coefficients.values[0], table_coefficients.values[1],
      table_coefficients.values[2], table_coefficients.values[3]);
  ROS_INFO("Step 3 done");

  // Step 4 : Project the table inliers on the table
  pcl::PointCloud<Point> table_projected;
  proj_.setInputCloud (cloud_downsampled_ptr);
  proj_.setIndices (table_inliers_ptr);
  proj_.setModelCoefficients (table_coefficients_ptr);
  proj_.filter (table_projected);
  pcl::PointCloud<Point>::ConstPtr table_projected_ptr =
    boost::make_shared<const pcl::PointCloud<Point> > (table_projected);
  ROS_INFO("Step 4 done");

  sensor_msgs::PointCloud table_points;
  tf::Transform table_plane_trans = getPlaneTransform (table_coefficients, up_direction_);
  //takes the points projected on the table and transforms them into the PointCloud message
  //while also transforming them into the table's coordinate system
  if (!getPlanePoints(table_projected, table_plane_trans, table_points))
  {
    return false;
  }
  ROS_INFO("Table computed");

  table = getTable(cloud.header, table_plane_trans, table_points);
  return true;
}