vector<PCPolygonPtr> PCPolyhedron::detectPolygons(const PCPtr& cloud, float planeTolerance, float pointsTolerance, bool limitFaces) { float DOT_EPSILON = 0.15; saveCloud("1_toDetect.pcd", *cloud); PCPtr cloudTemp(cloud); float maxFaces = limitFaces ? MAX_FACES : 100; sensor_msgs::PointCloud2::Ptr cloud_blob (new sensor_msgs::PointCloud2()); sensor_msgs::PointCloud2::Ptr cloud_filtered_blob (new sensor_msgs::PointCloud2); pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; vector<PCPolygonPtr> nuevos; PCPtr cloudP (new PC()); pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); pcl::SACSegmentation<pcl::PointXYZ> seg; pcl::ExtractIndices<pcl::PointXYZ> extract; std::vector<ofVec3f> vCloudHull; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (50); seg.setDistanceThreshold (planeTolerance); //original: 0.01 // Create the filtering object int i = 0, nrPoints = cloudTemp->points.size (); // mientras 7% de la nube no se haya procesad+o int numFaces = 0; while (cloudTemp->points.size () > 0.07 * nrPoints && numFaces < maxFaces) { pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloudTemp); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cerr << "Could not estimate a planar model for the given dataset." << std::endl; break; } //FIX PCPtr cloudFilteredTempInliers (new PC()); PCPtr cloudFilteredTempOutliers (new PC()); if(inliers->indices.size() != cloudTemp->size()) { // Extract the inliers extract.setInputCloud (cloudTemp); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloudFilteredTempInliers); cloudP = cloudFilteredTempInliers; } else cloudP = cloudTemp; // Create the filtering object extract.setInputCloud (cloudTemp); extract.setIndices (inliers); extract.setNegative (true); if(cloudP->size() != cloudTemp->size()) extract.filter (*cloudFilteredTempOutliers); cloudTemp = cloudFilteredTempOutliers; saveCloud("2_DetectedPol" + ofToString(i) + ".pcd", *cloudP); //Remove outliers by clustering vector<pcl::PointIndices> clusterIndices(findClusters(cloudP, pointsTolerance, 10, 10000)); int debuccount = 0; PCPtr cloudPFiltered (new PC()); if(clusterIndices.size() > 0) { cloudPFiltered = getCloudFromIndices(cloudP, clusterIndices.at(0)); } saveCloud("3_Postfilter_pol" + ofToString(i) + ".pcd",*cloudPFiltered); if (cloudPFiltered->size() < 4) break; //Controlo que las normales sean perpendiculares ofVec3f norm (coefficients->values[0],coefficients->values[1],coefficients->values[2]); norm.normalize(); bool normalCheck = true; for(int i = 0; i < nuevos.size() && normalCheck; i++) { float dot = abs(nuevos[i]->getNormal().dot(norm)); if( dot > DOT_EPSILON) { normalCheck = false; } } if(normalCheck) { //proyecto los puntos sobre el plano pcl::ProjectInliers<pcl::PointXYZ> proj; proj.setModelType(pcl::SACMODEL_PLANE); PCPtr projectedCloud (new PC()); proj.setInputCloud(cloudPFiltered); proj.setModelCoefficients(coefficients); proj.filter(*projectedCloud); saveCloud("4_Proy_Pol" + ofToString(i) + ".pcd",*projectedCloud); PCPolygonPtr pcp(new PCQuadrilateral(*coefficients, projectedCloud)); pcp->detectPolygon(); nuevos.push_back(pcp); numFaces++; } i++; } return nuevos; }
int main (int argc, char** argv) { sensor_msgs::PointCloud2::Ptr cloud_blob (new sensor_msgs::PointCloud2), cloud_filtered_blob (new sensor_msgs::PointCloud2); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>); // Fill in the cloud data pcl::PCDReader reader; reader.read ("table_scene_lms400.pcd", *cloud_blob); std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl; // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<sensor_msgs::PointCloud2> sor; sor.setInputCloud (cloud_blob); sor.setLeafSize (0.01f, 0.01f, 0.01f); sor.filter (*cloud_filtered_blob); // Convert to the templated PointCloud pcl::fromROSMsg (*cloud_filtered_blob, *cloud_filtered); std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl; // Write the downsampled version to disk pcl::PCDWriter writer; writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (1000); seg.setDistanceThreshold (0.01); // Create the filtering object pcl::ExtractIndices<pcl::PointXYZ> extract; int i = 0, nr_points = (int) cloud_filtered->points.size (); // While 30% of the original cloud is still there while (cloud_filtered->points.size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cerr << "Could not estimate a planar model for the given dataset." << std::endl; break; } // Extract the inliers extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloud_p); std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl; std::stringstream ss; ss << "table_scene_lms400_plane_" << i << ".pcd"; writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false); // Create the filtering object extract.setNegative (true); extract.filter (*cloud_filtered); i++; } return (0); }
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input) { pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>); pcl_conversions::toPCL(*input, *cloud_blob); // Fill in the cloud data //pcl::PCDReader reader; //reader.read ("table_scene_lms400.pcd", *cloud_blob); // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PCLPointCloud2> sor; sor.setInputCloud (cloud_blob); sor.setLeafSize (0.01f, 0.01f, 0.01f); sor.filter (*cloud_filtered_blob); // Convert to the templated PointCloud pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered); //std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl; // Write the downsampled version to disk //pcl::PCDWriter writer; //writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (1000); seg.setDistanceThreshold (0.01); // Create the filtering object pcl::ExtractIndices<pcl::PointXYZ> extract; // int i = 0, nr_points = (int) cloud_filtered->points.size (); // While 30% of the original cloud is still there //while (cloud_filtered->points.size () > 0.3 * nr_points) for (int i = 0; i < 2; i++) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { ROS_ERROR("Could not estimate a planar model for the given dataset."); break; } // Extract the inliers extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloud_p); // std::stringstream ss; // ss << "table_scene_lms400_plane_" << i << ".pcd"; // writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false); // Create the filtering object extract.setNegative (true); extract.filter (*cloud_f); cloud_filtered.swap (cloud_f); //i++; } //pcl::PCLPointCloud2 pre_output; sensor_msgs::PointCloud2 output; pcl::toROSMsg(*cloud_p, output); //pcl_conversions::fromPCL(pre_output, output); pub.publish(output); }
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input) { // Container for original & filtered data pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); // pcl::PCLPointCloud2 cloud_filtered; pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>); // Convert to PCL data type pcl_conversions::toPCL(*input, *cloud_blob); // Perform the actual filtering pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2); pcl::VoxelGrid<pcl::PCLPointCloud2> sor; sor.setInputCloud (cloud_blob); sor.setLeafSize (0.05, 0.05, 0.05); sor.setFilterFieldName("z"); sor.setFilterLimits(0.01, 0.3); sor.filter (*cloud_filtered_blob); // // Remove outlier X // pcl::PCLPointCloud2::Ptr cloud_filtered_blobx (new pcl::PCLPointCloud2); // pcl::VoxelGrid<pcl::PCLPointCloud2> sorx; // sorx.setInputCloud(cloud_filtered_blobz); // sorx.setFilterFieldName("x"); // sorx.setFilterLimits(-1, 1); // sorx.filter(*cloud_filtered_blobx); // // Remove outlier Y // pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2); // pcl::VoxelGrid<pcl::PCLPointCloud2> sory; // sory.setInputCloud(cloud_filtered_blobx); // sory.setFilterFieldName("y"); // sory.setFilterLimits(-1, 1); // sory.filter(*cloud_filtered_blob); // Convert to the templated PointCloud pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); Eigen::Vector3f upVector(0, 0, 1); seg.setAxis(upVector); seg.setEpsAngle(1.5708); seg.setMaxIterations (1000); seg.setDistanceThreshold (0.05); seg.setInputCloud (cloud_filtered); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) { std::cerr << "Could not estimate a planar model for the given dataset." << std::endl; return; } // Create the filtering object pcl::ExtractIndices<pcl::PointXYZ> extract; // Extract the inliers extract.setInputCloud (cloud_filtered); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloud_p); // Publish inliers // sensor_msgs::PointCloud2 inlierpc; // pcl_conversions::fromPCL(cloud_p, inlierpc); pub.publish (*cloud_p); // Publish the model coefficients pcl_msgs::ModelCoefficients ros_coefficients; pcl_conversions::fromPCL(*coefficients, ros_coefficients); pubCoef.publish (ros_coefficients); // ========================================== // // Convert to ROS data type // sensor_msgs::PointCloud2 downpc; // pcl_conversions::fromPCL(cloud_filtered, downpc); // // Publish the data // pub.publish (downpc); // pcl::ModelCoefficients coefficients; // pcl::PointIndices inliers; // //.makeShared() // // Create the segmentation object // pcl::SACSegmentation<pcl::PointXYZ> seg; // seg.setInputCloud (&cloudPtr); // seg.setOptimizeCoefficients (true); // Optional // seg.setModelType (pcl::SACMODEL_PLANE); // Mandatory // seg.setMethodType (pcl::SAC_RANSAC); // seg.setDistanceThreshold (0.1); // seg.segment (inliers, coefficients); // if (inliers.indices.size () == 0) // { // PCL_ERROR ("Could not estimate a planar model for the given dataset."); // return; // } // std::cerr << "Model coefficients: " << coefficients.values[0] << " " // << coefficients.values[1] << " " // << coefficients.values[2] << " " // << coefficients.values[3] << std::endl; // // Publish the model coefficients // pcl_msgs::ModelCoefficients ros_coefficients; // pcl_conversions::fromPCL(coefficients, ros_coefficients); // pubCoef.publish (ros_coefficients); }
void rgb_pcl::cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){ // Container for original & filtered data #if PR2 if(target_frame.find(base_frame) == std::string::npos){ getTransformCloud(input, *input); } sensor_msgs::PointCloud2 in = *input; sensor_msgs::PointCloud2 out; pcl_ros::transformPointCloud(target_frame, net_transform, in, out); #endif // ROS_INFO("Cloud acquired..."); pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>); Mat displayImage = cv::Mat(Size(640, 480), CV_8UC3); displayImage = Scalar(120); // Convert to PCL data type #if PR2 pcl_conversions::toPCL(out, *cloud); #endif #if !PR2 pcl_conversions::toPCL(*input, *cloud); #endif // ROS_INFO("\t=>Cloud rotated..."); // Perform the actual filtering pcl::VoxelGrid<pcl::PCLPointCloud2> sor; sor.setInputCloud (cloudPtr); sor.setLeafSize (0.005, 0.005, 0.005); sor.filter (*cloud_filtered_blob); pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered); ModelCoefficientsPtr coefficients (new pcl::ModelCoefficients); PointCloudPtr plane_points(new PointCloud), point_points_2d_hull(new PointCloud); std::vector<PointCloudPtr> object_clouds; pcl::PointCloud<pcl::PointXYZRGB> combinedCloud; #if PR2 make_crop_box_marker(marker_publisher, base_frame, 0, 0.2, -1, 0.2, 1.3, 2, 1.3); // Define your cube with two points in space: Eigen::Vector4f minPoint; minPoint[0]=0.2; // define minimum point x minPoint[1]=-1; // define minimum point y minPoint[2]=0.2; // define minimum point z Eigen::Vector4f maxPoint; maxPoint[0]=1.5; // define max point x maxPoint[1]=1; // define max point y maxPoint[2]=1.5; // define max point z pcl::CropBox<pcl::PointXYZRGB> cropFilter; cropFilter.setInputCloud (cloud_filtered); cropFilter.setMin(minPoint); cropFilter.setMax(maxPoint); cropFilter.filter (*cloud_filtered); #endif #if !PR2 //Rotate the point cloud Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity(); // Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix) float theta = M_PI; // The angle of rotation in radians // Define a translation of 0 meters on the x axis transform_1.translation() << 0.0, 0.0, 1.0; // The same rotation matrix as before; tetha radians arround X axis transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX())); // Executing the transformation pcl::transformPointCloud (*cloud_filtered, *cloud_filtered, transform_1); #endif interpretTableScene(cloud_filtered, coefficients, plane_points, point_points_2d_hull, object_clouds); int c = 0; #if PUBLISH_CLOUDS int ID_object = -1; #endif for(auto cloudCluster: object_clouds){ // get_cloud_matching(cloudCluster); //histogram matching #if PUBLISH_CLOUDS ID_object = c; #endif combinedCloud += *cloudCluster; combinedCloud.header = cloud_filtered->header; c++; } #if DISPLAY drawPointCloud(combinedCloud, displayImage); #endif getTracker(object_clouds, displayImage); stateDetection(); // ROS_INFO("\t=>Cloud analysed..."); #if PUBLISH_CLOUDS sensor_msgs::PointCloud2 output; if(object_clouds.size() >= ID_object && ID_object >= 0){ pcl::toROSMsg(combinedCloud, output); // Publish the data pub.publish (output); } #endif end = ros::Time::now(); std::stringstream ss; ss <<(end-begin); string s_FPS = ss.str(); #if DISPLAY cv::putText(displayImage, "FPS: "+to_string((int)1/(stof(s_FPS))) + " Desired: "+to_string(DESIRED_FPS), cv::Point(10, 10), CV_FONT_HERSHEY_COMPLEX, 0.4, Scalar(0,0,0)); imshow("RGB", displayImage); #endif waitKey(1); begin = ros::Time::now(); }
void cloud_cb (const pcl::PCLPointCloud2ConstPtr& cloud_blob) { std::cout<<"Received Kinect message\n"; pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>); // Create the filtering object: downsample the dataset using a leaf size of 1cm pcl::VoxelGrid<pcl::PCLPointCloud2> sor; sor.setInputCloud (cloud_blob); boost::this_thread::sleep (boost::posix_time::microseconds (100)); sor.setLeafSize (0.01f, 0.01f, 0.01f); sor.filter (*cloud_filtered_blob); // Convert to the templated PointCloud pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZRGB> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setMaxIterations (1000); seg.setDistanceThreshold (0.01); seg.setInputCloud (cloud_filtered); int i = 0, nr_points = (int) cloud_filtered->points.size (); pcl::IndicesPtr remaining (new std::vector<int>); remaining->resize (nr_points); for (size_t i = 0; i < remaining->size (); ++i) { (*remaining)[i] = static_cast<int>(i); } std::cout << "here again" << std::endl; // While 30% of the original cloud is still there while (remaining->size () > 0.3 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setIndices (remaining); seg.segment (*inliers, *coefficients); if (inliers->indices.size () == 0) break; // Extract the inliers std::vector<int>::iterator it = remaining->begin(); for (size_t i = 0; i < inliers->indices.size (); ++i) { int curr = inliers->indices[i]; // Remove it from further consideration. while (it != remaining->end() && *it < curr) { ++it; } if (it == remaining->end()) break; if (*it == curr) it = remaining->erase(it); } i++; } std::cout << "Found " << i << " planes." << std::endl; // Color all the non-planar things. for (std::vector<int>::iterator it = remaining->begin(); it != remaining->end(); ++it) { uint8_t r = 0, g = 255, b = 0; uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b); cloud_filtered->at(*it).rgb = *reinterpret_cast<float*>(&rgb); } pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer"); viewer.showCloud (cloud_filtered); while (!viewer.wasStopped ()) { } // Publish the planes we found. pcl::PCLPointCloud2 outcloud; pcl::toPCLPointCloud2 (*cloud_filtered, outcloud); pub.publish (outcloud); }