void App::Load() { Input_.reset(new PointCloud); pcl::io::loadPCDFile(InputPath_, *Input_); }
void GraphicEnd::downsamplePointCloud(PointCloud::Ptr& pc_in,PointCloud::Ptr& pc_downsampled) { if(use_voxel) { pcl::VoxelGrid<pcl::PointXYZRGB> grid; grid.setLeafSize(0.05,0.05,0.05); grid.setFilterFieldName ("z"); grid.setFilterLimits (0.0,5.0); grid.setInputCloud(pc_in); grid.filter(*pc_downsampled); } else { int downsamplingStep=8; static int j;j=0; std::vector<double> xV; std::vector<double> yV; std::vector<double> zV; std::vector<double> rV; std::vector<double> gV; std::vector<double> bV; pc_downsampled.reset(new pcl::PointCloud<pcl::PointXYZRGB> ); pc_downsampled->points.resize(640*480/downsamplingStep*downsamplingStep); for(int r=0;r<480;r=r+downsamplingStep) { for(int c=0;c<640;c=c+downsamplingStep) { int nPoints=0; xV.resize(downsamplingStep*downsamplingStep); yV.resize(downsamplingStep*downsamplingStep); zV.resize(downsamplingStep*downsamplingStep); rV.resize(downsamplingStep*downsamplingStep); gV.resize(downsamplingStep*downsamplingStep); bV.resize(downsamplingStep*downsamplingStep); for(int r2=r;r2<r+downsamplingStep;r2++) { for(int c2=c;c2<c+downsamplingStep;c2++) { //Check if the point has valid data if(pcl_isfinite (pc_in->points[r2*640+c2].x) && pcl_isfinite (pc_in->points[r2*640+c2].y) && pcl_isfinite (pc_in->points[r2*640+c2].z) && 0.3<pc_in->points[r2*640+c2].z && pc_in->points[r2*640+c2].z<5) { //Create a vector with the x, y and z coordinates of the square region and RGB info xV[nPoints]=pc_in->points[r2*640+c2].x; yV[nPoints]=pc_in->points[r2*640+c2].y; zV[nPoints]=pc_in->points[r2*640+c2].z; rV[nPoints]=pc_in->points[r2*640+c2].r; gV[nPoints]=pc_in->points[r2*640+c2].g; bV[nPoints]=pc_in->points[r2*640+c2].b; nPoints++; } } } if(nPoints>0) { xV.resize(nPoints); yV.resize(nPoints); zV.resize(nPoints); rV.resize(nPoints); gV.resize(nPoints); bV.resize(nPoints); //Compute the mean 3D point and mean RGB value std::sort(xV.begin(),xV.end()); std::sort(yV.begin(),yV.end()); std::sort(zV.begin(),zV.end()); std::sort(rV.begin(),rV.end()); std::sort(gV.begin(),gV.end()); std::sort(bV.begin(),bV.end()); pcl::PointXYZRGB point; point.x=xV[nPoints/2]; point.y=yV[nPoints/2]; point.z=zV[nPoints/2]; point.r=rV[nPoints/2]; point.g=gV[nPoints/2]; point.b=bV[nPoints/2]; //Set the mean point as the representative point of the region pc_downsampled->points[j]=point; j++; } } } pc_downsampled->points.resize(j); pc_downsampled->width=pc_downsampled->size(); pc_downsampled->height=1; } }
std::vector<PointCloud> TableClusterDetector::findTableClusters(const sensor_msgs::PointCloud2 &scene) { std::vector<PointCloud> clusters; // Convert the dataset PointCloud cloud; PointCloud::Ptr cloudPtr; pcl::fromROSMsg (scene, cloud); cloudPtr.reset(new PointCloud(cloud)); // Remove NaNs PointCloud cloud_filtered; pass_.setInputCloud (cloudPtr); pass_.filter (cloud_filtered); cloudPtr.reset(new PointCloud(cloud_filtered)); // Downsample PointCloud cloud_downsampled; grid_.setInputCloud (cloudPtr); grid_.filter (cloud_downsampled); cloudPtr.reset(new PointCloud(cloud_downsampled)); if ((int)cloud_filtered.points.size() < k_) { ROS_WARN("Filtering returned %zd points! Skipping.", cloud_filtered.points.size()); return clusters; } // Estimate the point normals pcl::PointCloud<pcl::Normal> cloud_normals;pcl::PointCloud<pcl::Normal>::Ptr cloud_normalsPtr; // add this if normal estimation is inaccurate //n3d_.setSearchSurface (cloud_); n3d_.setInputCloud (cloudPtr); n3d_.compute (cloud_normals); cloud_normalsPtr.reset(new pcl::PointCloud<pcl::Normal>(cloud_normals)); ROS_INFO ("[TableObjectDetector] %d normals estimated.", (int)cloud_normals.points.size ()); // ---[ Perform segmentation pcl::PointIndices table_inliers; pcl::PointIndices::Ptr table_inliersPtr; pcl::ModelCoefficients table_coefficients; pcl::ModelCoefficients::Ptr table_coefficientsPtr; seg_.setInputCloud (cloudPtr); seg_.setInputNormals (cloud_normalsPtr); seg_.segment (table_inliers, table_coefficients); table_inliersPtr = boost::make_shared<pcl::PointIndices>(table_inliers); table_coefficientsPtr = boost::make_shared<pcl::ModelCoefficients>(table_coefficients); if (table_coefficients.values.size () > 3) 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]); if (table_inliers.indices.size () == 0) return clusters; // ---[ Extract the table PointCloud table_projected; PointCloud::Ptr table_projectedPtr; proj_.setInputCloud (cloudPtr); proj_.setIndices (table_inliersPtr); proj_.setModelCoefficients (table_coefficientsPtr); proj_.filter (table_projected); table_projectedPtr.reset (new PointCloud(table_projected)); ROS_INFO ("[TableObjectDetector::input_callback] Number of projected inliers: %d.", (int)table_projected.points.size ()); sensor_msgs::PointCloud table_points; tf::Transform table_plane_trans = getPlaneTransform (*table_coefficientsPtr, -1.0); std::string base_frame_id = scene.header.frame_id; ROS_INFO("sending table transform"); ros::Rate r(10); for (int i=0; i < 10; i++) { tf_pub_.sendTransform(tf::StampedTransform(table_plane_trans, ros::Time::now(), base_frame_id, "table")); r.sleep(); } //takes the points projected on the table and transforms them into the PointCloud message //while also transforming them into the table's coordinate system getPlanePoints<Point> (table_projected, table_plane_trans, table_points); ROS_INFO("Table computed"); table_ = computeTable<sensor_msgs::PointCloud>(cloud.header, table_plane_trans, table_points); publishTable(table_); // ---[ Estimate the convex hull PointCloud table_hull; PointCloud::Ptr table_hullPtr; hull_.setInputCloud (table_projectedPtr); hull_.reconstruct (table_hull); table_hullPtr.reset (new PointCloud(table_hull)); // ---[ Get the objects on top of the table pcl::PointIndices cloud_object_indices; pcl::PointIndices::Ptr cloud_object_indicesPtr; prism_.setInputCloud (cloudPtr); prism_.setInputPlanarHull (table_hullPtr); prism_.segment (cloud_object_indices); cloud_object_indicesPtr = boost::make_shared<pcl::PointIndices>(cloud_object_indices); ROS_INFO ("[TableObjectDetector::input_callback] Number of object point indices: %d.", (int)cloud_object_indices.indices.size ()); PointCloud cloud_objects; PointCloud::Ptr cloud_objectsPtr; pcl::ExtractIndices<Point> extract_object_indices; extract_object_indices.setInputCloud (cloudPtr); extract_object_indices.setIndices (cloud_object_indicesPtr); extract_object_indices.filter (cloud_objects); cloudPtr.reset(new PointCloud(cloud_objects)); ROS_INFO ("[TableObjectDetector::input_callback] Number of object point candidates: %d.", (int)cloud_objects.points.size ()); if (cloud_objects.points.size () == 0) return clusters; // ---[ Downsample the points PointCloud cloud_objects_downsampled;PointCloud::Ptr cloud_objects_downsampledPtr; grid_objects_.setInputCloud (cloudPtr); grid_objects_.filter (cloud_objects_downsampled); cloudPtr.reset (new PointCloud(cloud_objects_downsampled)); ROS_INFO ("[TableObjectDetector::input_callback] Number of object point candidates left after downsampling: %d.", (int)cloud_objects_downsampled.points.size ()); // ---[ Split the objects into Euclidean clusters std::vector<pcl::PointIndices> clustersIndices; cluster_.setInputCloud (cloudPtr); cluster_.extract (clustersIndices); ROS_INFO ("[TableObjectDetector::input_callback] Number of clusters found matching the given constraints: %d.", (int)clustersIndices.size ()); AffineTransform table_trans_eig; cse481::tfToEigen(table_plane_trans, table_trans_eig); table_trans_eig = table_trans_eig.inverse().eval(); PointCloud cloud_in_table_frame; pcl::transformPointCloud(cloud_objects_downsampled, cloud_in_table_frame, table_trans_eig); // Clouds are now in table frame BOOST_FOREACH(pcl::PointIndices indices, clustersIndices) { PointCloud clusterCloud; pcl::copyPointCloud(cloud_in_table_frame, indices, clusterCloud); clusters.push_back(clusterCloud); }