RG_Segmenter.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package VisionX
19  * @author Eren Aksoy ( eren dot aksoy at kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "RG_Segmenter.h"
26 
27 RGSegClass::RGSegClass()
28 {
29  /// Create variables needed for preparations
30  input_cloud_ptr.reset(new pcl::PointCloud<PointT>);
31  output_colored_cloud.reset(new pcl::PointCloud<PointT>);
32  output_labeled_cloud.reset(new pcl::PointCloud<PointL>);
33 
34  SmoothnessThreshold = 30.0f;
35  CurvatureThreshold = 10.0f;
36 }
37 
38 //---------------------------------------------------------------------------------------------------------------------//
39 
40 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr&
41 RGSegClass::ApplyRegionGrowingSegmentation(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& CloudPtr)
42 {
43  // copy original points to the cloud
44  pcl::copyPointCloud(*CloudPtr, *input_cloud_ptr);
45 
46  RegionGrowingSegmenter();
47 
48  return output_colored_cloud;
49 }
50 
51 //---------------------------------------------------------------------------------------------------------------------//
52 
53 void
54 RGSegClass::RegionGrowingSegmenter()
55 {
56  pcl::search::Search<PointT>::Ptr tree =
57  pcl::search::Search<PointT>::Ptr(new pcl::search::KdTree<PointT>);
58  pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
59  pcl::NormalEstimation<PointT, pcl::Normal> normal_estimator;
60  normal_estimator.setSearchMethod(tree);
61  normal_estimator.setInputCloud(input_cloud_ptr);
62  normal_estimator.setKSearch(50);
63  normal_estimator.compute(*normals);
64 
65  /*
66  pcl::IndicesPtr indices (new std::vector <int>);
67  pcl::PassThrough<PointT> pass;
68  pass.setInputCloud (input_cloud_ptr);
69  pass.setFilterFieldName ("y");
70  pass.setFilterLimits (0.0, 5000.0);
71  pass.filter (*indices);
72  */
73 
74  pcl::RegionGrowing<PointT, pcl::Normal> reg;
75  reg.setMinClusterSize(50);
76  reg.setMaxClusterSize(1000000);
77  reg.setSearchMethod(tree);
78  reg.setNumberOfNeighbours(25);
79  reg.setInputCloud(input_cloud_ptr);
80  //reg.setIndices (indices);
81  reg.setInputNormals(normals);
82  reg.setSmoothnessThreshold(SmoothnessThreshold / 180.0 * M_PI);
83  reg.setCurvatureThreshold(CurvatureThreshold);
84 
85  std::vector<pcl::PointIndices> clusters;
86  reg.extract(clusters);
87 
88  std::cout << "=RG Segmenter: Number of clusters is " << clusters.size() << std::endl;
89 
90  output_colored_cloud = reg.getColoredCloudRGBA();
91 }
92 
93 //---------------------------------------------------------------------------------------------------------------------//
94 
95 void
96 RGSegClass::UpdateRegionGrowingSegmentationParameters(float SmoothnessThres, float CurvatureThres)
97 {
98  SmoothnessThreshold = SmoothnessThres;
99  CurvatureThreshold = CurvatureThres;
100 
101  std::cout << "=RG Segmenter: Parameters updated! >> SmoothnessThreshold : "
102  << SmoothnessThreshold << " CurvatureThreshold: " << CurvatureThreshold << std::endl;
103 }
104 
105 //---------------------------------------------------------------------------------------------------------------------//
106 
107 void
108 RGSegClass::ConvertFromXYZRGBAtoXYZL()
109 {
110  // to be done!...
111 }
112 
113 //---------------------------------------------------------------------------------------------------------------------//
RGSegClass::RGSegClass
RGSegClass()
Definition: RG_Segmenter.cpp:27
M_PI
#define M_PI
Definition: MathTools.h:17
RGSegClass::ApplyRegionGrowingSegmentation
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr & ApplyRegionGrowingSegmentation(pcl::PointCloud< pcl::PointXYZRGBA >::Ptr &CloudPtr)
Definition: RG_Segmenter.cpp:41
RGSegClass::UpdateRegionGrowingSegmentationParameters
void UpdateRegionGrowingSegmentationParameters(float SmoothnessThres, float CurvatureThres)
Definition: RG_Segmenter.cpp:96
RG_Segmenter.h