int main (int argc, char** argv) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_original(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_pass_filter0(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_pass_filter1(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_pass_filter2(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2_planar(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud3_non_planar(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud4_cylinder(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud4_cylinder_filtered(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud5_non_planar_non_cylinder(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PCDReader reader; //reader.read<pcl::PointXYZRGB> ("/home/niladri-64/plc_kinect_workspace/robot/build/two_objects/saved_file4.pcd",*cloud_original); reader.read<pcl::PointXYZRGB> ("/home/niladri-64/plc_kinect_workspace/openni_grabber/build/saved_file2.pcd",*cloud_original); // TRansforming Eigen::Matrix4f temp_trans; temp_trans << -0.819538 , -0.26171 , 0.50977, -0.316881 , -0.572858, 0.352719, -0.73988 ,0.944997 , 0.013828, -0.898386, -0.438989 , 0.737386, 0, 0, 0, 1; pcl::transformPointCloud(*cloud_original, *cloud, temp_trans ); { boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; viewer = rgbVis(cloud); while (!viewer->wasStopped()) { viewer->spinOnce(1000); } } pcl::PassThrough<pcl::PointXYZRGB> pass; pass.setFilterFieldName ("z"); pass.setFilterLimits (-0.18, 0.5); pass.setInputCloud (cloud); pass.filter (*result_pass_filter0); pass.setFilterFieldName ("x"); pass.setFilterLimits (0.77, 1.2); pass.setInputCloud (result_pass_filter0); pass.filter (*result_pass_filter1); pass.setFilterFieldName ("y"); pass.setFilterLimits (-0.5, 0.5); pass.setInputCloud (result_pass_filter1); pass.filter (*result_pass_filter2); { boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; viewer = rgbVis(result_pass_filter2); while (!viewer->wasStopped()) { viewer->spinOnce(1000); } } // Planar Segmentation pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); 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.02); // Create the filtering object pcl::ExtractIndices<pcl::PointXYZRGB> extract; int i = 0, nr_points = (int) result_pass_filter2->points.size (); // While 30% of the original cloud is still there while (result_pass_filter2->points.size () > 0.5 * nr_points) { // Segment the largest planar component from the remaining cloud seg.setInputCloud (result_pass_filter2); seg.segment (*inliers, *coefficients); // std::cerr << "Model coefficients: " << coefficients->values[0] << " " // << coefficients->values[1] << " " // << coefficients->values[2] << " " // << coefficients->values[3] << std::endl; 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 (result_pass_filter2); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloud2_planar); std::cerr << "PointCloud representing the planar component: " << cloud2_planar->width * cloud2_planar->height << " data points." << std::endl; extract.setNegative (true); extract.filter (*cloud3_non_planar); std::stringstream ss; ss << "table_scene_lms400_plane_" << i << ".pcd"; //writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false); { boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; viewer = viewportsVis(cloud3_non_planar, result_pass_filter2); while (!viewer->wasStopped()) { viewer->spinOnce (1000); } } i++; result_pass_filter2.swap (cloud3_non_planar); } // while loop result_pass_filter2.swap (cloud3_non_planar); // cloud3_non_planar is again the non-planar component { // cylinder segmentation pcl::PointCloud<pcl::Normal>::Ptr cloud_normals3 (new pcl::PointCloud<pcl::Normal>); pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ()); pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne; // Estimate point normals ne.setSearchMethod (tree); ne.setInputCloud (cloud3_non_planar); ne.setKSearch (50); ne.compute (*cloud_normals3); pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); // pcl::SACSegmentation<pcl::PointXYZRGB> seg2; pcl::SACSegmentationFromNormals<pcl::PointXYZRGB, pcl::Normal> seg2; // Optional seg2.setOptimizeCoefficients (true); seg2.setModelType (pcl::SACMODEL_CYLINDER); seg2.setMethodType (pcl::SAC_RANSAC); seg2.setNormalDistanceWeight (0.1); seg2.setMaxIterations (10000); seg2.setDistanceThreshold (0.07); seg2.setRadiusLimits (0, 0.11); seg2.setInputCloud (cloud3_non_planar); seg2.setInputNormals (cloud_normals3); // Create the filtering object pcl::ExtractIndices<pcl::PointXYZRGB> extract; int i = 0, nr_points = (int) cloud3_non_planar->points.size (); // While 30% of the original cloud is still there // while (result2->points.size () > 0.5 * nr_points) // { // Segment the largest planar component from the remaining cloud seg2.setInputCloud (cloud3_non_planar); seg2.segment (*inliers, *coefficients); std::cerr << "Model coefficients of Cylinder : " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << " " << coefficients->values[4] << " " << coefficients->values[5] << " " << coefficients->values[6] << " " <<std::endl; std::cout << "Orientation :" << coefficients->values[3] << " " << coefficients->values[4] << " "<< coefficients->values[5] << std::endl; 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 (cloud3_non_planar); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*cloud4_cylinder); std::cerr << "PointCloud representing the planar component: " << cloud4_cylinder->width * cloud4_cylinder->height << " data points." << std::endl; std::stringstream ss; ss << "table_scene_lms400_plane_" << i << ".pcd"; //writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false); { // visualizer boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; viewer = viewportsVis(cloud4_cylinder, cloud3_non_planar); while (!viewer->wasStopped()) { viewer->spinOnce (1000); } } // Create the filtering object extract.setNegative (true); extract.filter (*cloud5_non_planar_non_cylinder); // cloud3_non_planar.swap (cloud5_non_planar_non_cylinder); we are not running a loop //i++; } // cylinder segmentation () pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor; sor.setInputCloud (cloud4_cylinder); sor.setMeanK (50); sor.setStddevMulThresh (1.0); sor.filter (*cloud4_cylinder_filtered); { boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer; viewer = viewportsVis(cloud4_cylinder_filtered, cloud4_cylinder); while (!viewer->wasStopped()) { viewer->spinOnce (1000); } } pcl::PointXYZRGB minPt, maxPt; pcl::getMinMax3D (*cloud4_cylinder_filtered, minPt, maxPt); std::cout << "Max x: " << maxPt.x << std::endl; std::cout << "Max y: " << maxPt.y << std::endl; std::cout << "Max z: " << maxPt.z << std::endl; std::cout << "Min x: " << minPt.x << std::endl; std::cout << "Min y: " << minPt.y << std::endl; std::cout << "Min z: " << minPt.z << std::endl; std::cout << "Position :" << minPt.x << " " << (maxPt.y + minPt.y)/2 << " "<< (maxPt.z + minPt.z)/2 << std::endl; //std::cout << "Orientation :" << coefficients->values[4] << " " << coefficients->values[5] << " "<< coefficients->values[6] << std::endl; return 0; }
int main(int argc, char** argv) { // initialize variables pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_original (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_unaltered (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_objects (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_planeInliers (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_planeOutliers (new pcl::PointCloud<pcl::PointXYZ>); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients()); pcl::PointIndices::Ptr indices (new pcl::PointIndices()); pcl::PCDReader reader; pcl::PCDWriter writer; int result = 0; // catch function return // enable variables for time logging boost::posix_time::ptime time_before_execution; boost::posix_time::ptime time_after_execution; boost::posix_time::time_duration difference; // read point cloud through command line if (argc != 2) { std::cout << "usage: " << argv[0] << " <filename>\n"; return 0; } else { // read cloud and display its size std::cout << "Reading Point Cloud" << std::endl; reader.read(argv[1], *cloud_original); #ifdef DEBUG std::cout << "size of original cloud: " << cloud_original->points.size() << " points" << std::endl; #endif } //************************************************************************** // test passthroughFilter() function #ifdef PASSTHROUGH_FILTER_TEST std::cout << "RUNNING PASSTHROUGH FILTER TESTS" << std::endl; log_passthroughFilter(cloud_original, cloud_filtered, "../pictures/T01_passthrough_01.pcd", "passthrough filter, custom 1: ","z", -2.5, 0); log_passthroughFilter(cloud_original, cloud_filtered, "../pictures/T01_passthrough_02.pcd", "passthrough filter, custom 2: ","y", -3.0, 3.0); log_passthroughFilter(cloud_original, cloud_filtered, "../pictures/T01_passthrough_03.pcd", "passthrough filter, custom 3: ","x", -3.0, 3.0); #endif //************************************************************************** // test voxelFilter() function #ifdef VOXEL_FILTER_TEST std::cout << "RUNNING VOXEL FILTER LEAF SIZE TEST" << std::endl; test_voxelFilter_leafSize(cloud_original); #endif //************************************************************************** // test removeNoise() function #ifdef NOISE_REMOVAL_TEST std::cout << "RUNNING NOISE REMOVAL NEAREST NEIGHBORS TEST" << std::endl; test_removeNoise_neighbors(cloud_original); std::cout << "RUNNING NOISE REMOVAL THRESHOLD TEST" << std::endl; test_removeNoise_stdDeviation(cloud_original); #endif //************************************************************************** // test getPrism() function #ifdef EXTRACT_PRISM_DATA_TEST std::cout << "RUNNING EXTRACT PRISM ITERATIONS TEST" << std::endl; test_getPrism_iterations(cloud_original); std::cout << "RUNNING EXTRACT PRISM THRESHOLD TEST" << std::endl; test_getPrism_threshold(cloud_original); #endif //************************************************************************** // test objectSegmentation() function #ifdef SEGMENT_OBJECTS_TEST std::cout << "RUNNING OBJECT SEGMENTATION TEST" << std::endl; test_segmentObjects(cloud_original); #endif //************************************************************************** // test objectSegmentation() function #ifdef EUCLIDEAN_CLUSTER_TEST std::cout << "RUNNING EUCLIDEAN CLUSTER TEST" << std::endl; test_getClusters(cloud_original); #endif return 0; }
int main(int argc, char** argv) { // initialize variables pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_original (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_unaltered (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_objects (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_planeInliers (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_planeOutliers (new pcl::PointCloud<pcl::PointXYZ>); pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients()); pcl::PointIndices::Ptr indices (new pcl::PointIndices()); pcl::PCDReader reader; pcl::PCDWriter writer; int result = 0; // catch function return // enable variables for time logging boost::posix_time::ptime time_before_execution; boost::posix_time::ptime time_after_execution; boost::posix_time::time_duration difference; // read point cloud through command line if (argc != 2) { std::cout << "usage: " << argv[0] << " <filename>\n"; return 0; } else { // read cloud and display its size std::cout << "Reading Point Cloud" << std::endl; reader.read(argv[1], *cloud_original); #ifdef DEBUG std::cout << "size of original cloud: " << cloud_original->points.size() << " points" << std::endl; #endif } //************************************************************************** // test passthroughFilter() function std::cout << "RUNNING PASSTHROUGH FILTER TESTS" << std::endl; log_passthroughFilter(cloud_original, cloud_filtered, "../pictures/T01_passthrough_01.pcd", "passthrough filter, custom 1: ","z", -2.5, 0); log_passthroughFilter(cloud_original, cloud_filtered, "../pictures/T01_passthrough_02.pcd", "passthrough filter, custom 2: ","y", -3.0, 3.0); log_passthroughFilter(cloud_original, cloud_filtered, "../pictures/T01_passthrough_03.pcd", "passthrough filter, custom 3: ","x", -3.0, 3.0); //************************************************************************** // test voxelFilter() function std::cout << "RUNNING VOXEL FILTER TEST" << std::endl; log_voxelFilter(cloud_original, cloud_filtered, "../pictures/T02_voxel_01.pcd", "voxel filter, custom 1:", 0.01); log_voxelFilter(cloud_original, cloud_filtered, "../pictures/T02_voxel_02.pcd", "voxel filter, custom 2:", 0.02); log_voxelFilter(cloud_original, cloud_filtered, "../pictures/T02_voxel_03.pcd", "voxel filter, custom 3:", 0.04); log_voxelFilter(cloud_original, cloud_filtered, "../pictures/T02_voxel_04.pcd", "voxel filter, custom 4:", 0.08); //************************************************************************** // test removeNoise() function std::cout << "RUNNING NOISE REMOVAL TEST" << std::endl; log_removeNoise(cloud_original, cloud_filtered, "../pictures/T03_noise_01.pcd", "noise removal, custom 1: ", 50, 1.0); log_removeNoise(cloud_original, cloud_filtered, "../pictures/T03_noise_01.pcd", "noise removal, custom 1: ", 100, 1.0); log_removeNoise(cloud_original, cloud_filtered, "../pictures/T03_noise_01.pcd", "noise removal, custom 1: ", 10, 1.0); log_removeNoise(cloud_original, cloud_filtered, "../pictures/T03_noise_01.pcd", "noise removal, custom 1: ", 50, 1.9); log_removeNoise(cloud_original, cloud_filtered, "../pictures/T03_noise_01.pcd", "noise removal, custom 1: ", 50, 0.1); //************************************************************************** // test getPlane() function std::cout << "RUNNING PLANE SEGMENTATION TEST" << std::endl; log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers, coefficients, indices, "../pictures/T04_planeInliers_01.pcd", "plane segmentation, custom 1: ", 0, 1.57, 1000, 0.01); log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers, coefficients, indices, "../pictures/T04_planeInliers_02.pcd", "plane segmentation, custom 2: ", 0, 1.57, 1, 0.01); log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers, coefficients, indices, "../pictures/T04_planeInliers_03.pcd", "plane segmentation, custom 3: ", 0, 1.57, 2000, 0.01); log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers, coefficients, indices, "../pictures/T04_planeInliers_04.pcd", "plane segmentation, custom 4: ", 0, 0.76, 1000, 0.01); log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers, coefficients, indices, "../pictures/T04_planeInliers_05.pcd", "plane segmentation, custom 5: ", 0, 2.09, 1000, 0.01); log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers, coefficients, indices, "../pictures/T04_planeInliers_06.pcd", "plane segmentation, custom 6: ", 0.35, 1.57, 1000, 0.01); log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers, coefficients, indices, "../pictures/T04_planeInliers_07.pcd", "plane segmentation, custom 7: ", 0.79, 1.57, 1000, 0.01); log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers, coefficients, indices, "../pictures/T04_planeInliers_08.pcd", "plane segmentation, custom 8: ", 0, 1.57, 1000, 0.001); log_getPlane(cloud_original, cloud_planeInliers, cloud_planeOutliers, coefficients, indices, "../pictures/T04_planeInliers_09.pcd", "plane segmentation, custom 9: ", 0, 1.57, 1000, 0.01); //************************************************************************** // test getPrism() function std::cout << "RUNNING EXTRACT PRISM TEST" << std::endl; log_getPrism(cloud_original, cloud_objects, "../pictures/T05_objects_01.pcd", "polygonal prism, custom 1: ", 0, 1.57, 1000, 0.01, 0.02, 0.2); // test getPrism() function std::cout << "RUNNING EXTRACT PRISM TEST" << std::endl; // get output from default plane time_before_execution = boost::posix_time::microsec_clock::local_time(); // time before result = c44::getPrism(cloud_original, cloud_objects, 0, 1.57, 1000, 0.01, 0.02, 0.2); time_after_execution = boost::posix_time::microsec_clock::local_time(); // time after if(result < 0) { std::cout << "ERROR: could not find polygonal prism data" << std::endl; } else { writer.write<pcl::PointXYZ> ("../pictures/T05_objects_01.pcd", *cloud_objects); // write out cloud #ifdef DEBUG difference = time_after_execution - time_before_execution; // get execution time std::cout << std::setw(5) << difference.total_milliseconds() << ": " << "polygonal prism, default: " << cloud_objects->points.size() << " points" << std::endl; #endif } return 0; }