Go to the documentation of this file.00001 #include <pcl/point_types.h>
00002 #include <pcl/io/pcd_io.h>
00003 #include <pcl/console/time.h>
00004
00005 #include <pcl/filters/voxel_grid.h>
00006 #include <pcl/features/normal_3d.h>
00007 #include <pcl/segmentation/conditional_euclidean_clustering.h>
00008
00009 typedef pcl::PointXYZI PointTypeIO;
00010 typedef pcl::PointXYZINormal PointTypeFull;
00011
00012 bool
00013 enforceIntensitySimilarity (const PointTypeFull& point_a, const PointTypeFull& point_b, float squared_distance)
00014 {
00015 if (fabs (point_a.intensity - point_b.intensity) < 5.0f)
00016 return (true);
00017 else
00018 return (false);
00019 }
00020
00021 bool
00022 enforceCurvatureOrIntensitySimilarity (const PointTypeFull& point_a, const PointTypeFull& point_b, float squared_distance)
00023 {
00024 Eigen::Map<const Eigen::Vector3f> point_a_normal = point_a.normal, point_b_normal = point_b.normal;
00025 if (fabs (point_a.intensity - point_b.intensity) < 5.0f)
00026 return (true);
00027 if (fabs (point_a_normal.dot (point_b_normal)) < 0.05)
00028 return (true);
00029 return (false);
00030 }
00031
00032 bool
00033 customRegionGrowing (const PointTypeFull& point_a, const PointTypeFull& point_b, float squared_distance)
00034 {
00035 Eigen::Map<const Eigen::Vector3f> point_a_normal = point_a.normal, point_b_normal = point_b.normal;
00036 if (squared_distance < 10000)
00037 {
00038 if (fabs (point_a.intensity - point_b.intensity) < 8.0f)
00039 return (true);
00040 if (fabs (point_a_normal.dot (point_b_normal)) < 0.06)
00041 return (true);
00042 }
00043 else
00044 {
00045 if (fabs (point_a.intensity - point_b.intensity) < 3.0f)
00046 return (true);
00047 }
00048 return (false);
00049 }
00050
00051 int
00052 main (int argc, char** argv)
00053 {
00054
00055 pcl::PointCloud<PointTypeIO>::Ptr cloud_in (new pcl::PointCloud<PointTypeIO>), cloud_out (new pcl::PointCloud<PointTypeIO>);
00056 pcl::PointCloud<PointTypeFull>::Ptr cloud_with_normals (new pcl::PointCloud<PointTypeFull>);
00057 pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters), small_clusters (new pcl::IndicesClusters), large_clusters (new pcl::IndicesClusters);
00058 pcl::search::KdTree<PointTypeIO>::Ptr search_tree (new pcl::search::KdTree<PointTypeIO>);
00059 pcl::console::TicToc tt;
00060
00061
00062 std::cerr << "Loading...\n", tt.tic ();
00063 pcl::io::loadPCDFile ("Statues_4.pcd", *cloud_in);
00064 std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_in->points.size () << " points\n";
00065
00066
00067 std::cerr << "Downsampling...\n", tt.tic ();
00068 pcl::VoxelGrid<PointTypeIO> vg;
00069 vg.setInputCloud (cloud_in);
00070 vg.setLeafSize (80.0, 80.0, 80.0);
00071 vg.setDownsampleAllData (true);
00072 vg.filter (*cloud_out);
00073 std::cerr << ">> Done: " << tt.toc () << " ms, " << cloud_out->points.size () << " points\n";
00074
00075
00076 std::cerr << "Computing normals...\n", tt.tic ();
00077 pcl::copyPointCloud (*cloud_out, *cloud_with_normals);
00078 pcl::NormalEstimation<PointTypeIO, PointTypeFull> ne;
00079 ne.setInputCloud (cloud_out);
00080 ne.setSearchMethod (search_tree);
00081 ne.setRadiusSearch (300.0);
00082 ne.compute (*cloud_with_normals);
00083 std::cerr << ">> Done: " << tt.toc () << " ms\n";
00084
00085
00086 std::cerr << "Segmenting to clusters...\n", tt.tic ();
00087 pcl::ConditionalEuclideanClustering<PointTypeFull> cec (true);
00088 cec.setInputCloud (cloud_with_normals);
00089 cec.setConditionFunction (&customRegionGrowing);
00090 cec.setClusterTolerance (500.0);
00091 cec.setMinClusterSize (cloud_with_normals->points.size () / 1000);
00092 cec.setMaxClusterSize (cloud_with_normals->points.size () / 5);
00093 cec.segment (*clusters);
00094 cec.getRemovedClusters (small_clusters, large_clusters);
00095 std::cerr << ">> Done: " << tt.toc () << " ms\n";
00096
00097
00098 for (int i = 0; i < small_clusters->size (); ++i)
00099 for (int j = 0; j < (*small_clusters)[i].indices.size (); ++j)
00100 cloud_out->points[(*small_clusters)[i].indices[j]].intensity = -2.0;
00101 for (int i = 0; i < large_clusters->size (); ++i)
00102 for (int j = 0; j < (*large_clusters)[i].indices.size (); ++j)
00103 cloud_out->points[(*large_clusters)[i].indices[j]].intensity = +10.0;
00104 for (int i = 0; i < clusters->size (); ++i)
00105 {
00106 int label = rand () % 8;
00107 for (int j = 0; j < (*clusters)[i].indices.size (); ++j)
00108 cloud_out->points[(*clusters)[i].indices[j]].intensity = label;
00109 }
00110
00111
00112 std::cerr << "Saving...\n", tt.tic ();
00113 pcl::io::savePCDFile ("output.pcd", *cloud_out);
00114 std::cerr << ">> Done: " << tt.toc () << " ms\n";
00115
00116 return (0);
00117 }