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();
}
}
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;
}
