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