tools/ExtractObject/main.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <pcl/point_types.h>
29 #include <pcl/point_cloud.h>
30 #include <pcl/io/pcd_io.h>
31 #include <pcl/ModelCoefficients.h>
32 #include <pcl/common/common.h>
33 #include <pcl/common/pca.h>
34 #include <pcl/filters/extract_indices.h>
35 #include <pcl/segmentation/sac_segmentation.h>
36 #include <pcl/segmentation/extract_clusters.h>
37 
38 #include <rtabmap/utilite/UConversion.h>
39 
40 void showUsage()
41 {
42  printf("\nUsage: extractObject [options] cloud.pcd\n"
43  "Options:\n"
44  " -p #.# plane distance threshold (default 0.02 m)\n"
45  " -a #.# plane angle tolerance from z axis (default PI/6)\n"
46  " -c #.# cluster tolerance (default 0.1 m)\n"
47  " -s # minimum cluster size (default 50 points)\n"
48  " -center_obj center the objects to their local reference\n"
49  " -save_plane save the plane inliers to \"extracted_plane.pcd\"\n");
50 }
51 
52 int main(int argc, char *argv[])
53 {
54  if(argc < 2)
55  {
56  showUsage();
57  return -1;
58  }
59 
60  std::string cloudPath = argv[argc-1];
61  double planeDistanceThreshold = 0.02f;
62  double planeEpsAngle = 3.1416/6.0;
63  double clusterTolerance = 0.1f;
64  int minClusterSize = 50;
65  bool centerObject = false;
66  bool savePlane = false;
67 
68  for(int i=1; i<argc-1; ++i)
69  {
70  if(strcmp(argv[i], "-p") == 0)
71  {
72  ++i;
73  if(i < argc)
74  {
75  planeDistanceThreshold = uStr2Float(argv[i]);
76  if(planeDistanceThreshold < 0.0f)
77  {
78  showUsage();
79  }
80  }
81  else
82  {
83  showUsage();
84  }
85  continue;
86  }
87  if(strcmp(argv[i], "-a") == 0)
88  {
89  ++i;
90  if(i < argc)
91  {
92  planeEpsAngle = uStr2Float(argv[i]);
93  if(planeEpsAngle < 0.0f)
94  {
95  showUsage();
96  }
97  }
98  else
99  {
100  showUsage();
101  }
102  continue;
103  }
104  if(strcmp(argv[i], "-c") == 0)
105  {
106  ++i;
107  if(i < argc)
108  {
109  clusterTolerance = uStr2Float(argv[i]);
110  if(clusterTolerance <= 0.0f)
111  {
112  showUsage();
113  }
114  }
115  else
116  {
117  showUsage();
118  }
119  continue;
120  }
121  if(strcmp(argv[i], "-s") == 0)
122  {
123  ++i;
124  if(i < argc)
125  {
126  minClusterSize = std::atoi(argv[i]);
127  if(minClusterSize < 1)
128  {
129  showUsage();
130  }
131  }
132  else
133  {
134  showUsage();
135  }
136  continue;
137  }
138  if(strcmp(argv[i], "-center_obj") == 0)
139  {
140  centerObject = true;
141  continue;
142  }
143  if(strcmp(argv[i], "-save_plane") == 0)
144  {
145  savePlane = true;
146  continue;
147  }
148  printf("Unrecognized option : %s\n", argv[i]);
149  showUsage();
150  }
151 
152  printf("Parameters:\n"
153  " planeDistanceThreshold=%f\n"
154  " planeEpsAngle=%f\n"
155  " clusterTolerance=%f\n"
156  " minClusterSize=%d\n"
157  " centerObject=%s\n"
158  " savePlane=%s\n",
159  planeDistanceThreshold,
160  planeEpsAngle,
161  clusterTolerance,
162  minClusterSize,
163  centerObject?"true":"false",
164  savePlane?"true":"false");
165 
166  printf("Loading \"%s\"...", cloudPath.c_str());
167  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
168  pcl::io::loadPCDFile(cloudPath, *cloud);
169  printf("done! (%d points)\n", (int)cloud->size());
170 
171  // Extract plane
172  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
173  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
174  // Create the segmentation object
175  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
176  // Optional
177  seg.setOptimizeCoefficients (true);
178  // Mandatory
179  seg.setModelType (pcl::SACMODEL_PLANE);
180  seg.setMethodType (pcl::SAC_RANSAC);
181  seg.setMaxIterations(1000);
182  seg.setDistanceThreshold (planeDistanceThreshold);
183  seg.setAxis(Eigen::Vector3f(0,0,1));
184  seg.setEpsAngle (planeEpsAngle);
185 
186  seg.setInputCloud (cloud);
187  seg.segment (*inliers, *coefficients);
188 
189  printf("Plane coefficients: %f %f %f %f\n", coefficients->values[0],
190  coefficients->values[1],
191  coefficients->values[2],
192  coefficients->values[3]);
193 
194  float a = coefficients->values[0];
195  float b = coefficients->values[1];
196  float c = coefficients->values[2];
197  float d = coefficients->values[3];
198 
199  // Create a quaternion for rotation into XY plane
200  Eigen::Vector3f current(a, b, c);
201  Eigen::Vector3f target(0.0, 0.0, 1.0);
202  Eigen::Quaternion<float> q;
203  q.setFromTwoVectors(current, target);
204  Eigen::Matrix4f trans;
205  trans.topLeftCorner<3,3>() = q.toRotationMatrix();
206 
207  float planeShift = -d;
208 
209  // Create transformed point cloud (polygon is aligned with XY plane)
210  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB> ());
211  pcl::transformPointCloud(*cloud, *output, Eigen::Vector3f(-a*planeShift, -b*planeShift, -c*planeShift), q);
212 
213  if(savePlane)
214  {
215  pcl::io::savePCDFile("extracted_plane.pcd", *output, inliers->indices);
216  printf("Saved extracted_plane.pcd (%d points)\n", (int)inliers->indices.size());
217  }
218  else
219  {
220  printf("Plane size = %d points\n", (int)inliers->indices.size());
221  }
222 
223  // remove plane inliers
224  pcl::ExtractIndices<pcl::PointXYZRGB> extract;
225  extract.setNegative (true);
226  extract.setInputCloud (output);
227  extract.setIndices(inliers);
228  extract.filter (*output);
229 
230  // Get the biggest cluster
231  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZRGB>);
232  kdTree->setInputCloud(output);
233  std::vector<pcl::PointIndices> cluster_indices;
234  pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
235  ec.setClusterTolerance (clusterTolerance);
236  ec.setMinClusterSize (minClusterSize);
237  ec.setMaxClusterSize (200000);
238  ec.setSearchMethod (kdTree);
239  ec.setInputCloud (output);
240  ec.extract (cluster_indices);
241 
242  if(cluster_indices.size() == 0)
243  {
244  printf("No object found! (minimum cluster size=%d)\n", minClusterSize);
245  return 1;
246  }
247 
248  for(unsigned int i=0; i<cluster_indices.size(); ++i)
249  {
250  pcl::PointCloud<pcl::PointXYZRGB>::Ptr object (new pcl::PointCloud<pcl::PointXYZRGB> ());
251  pcl::copyPointCloud(*output, cluster_indices.at(i), *object);
252 
253  if(centerObject)
254  {
255  // recenter the object on xy plane, and set min z to 0
256  Eigen::Vector4f min, max;
257  pcl::getMinMax3D(*object, min, max);
258 
259  pcl::transformPointCloud(*object, *object, Eigen::Vector3f(-(max[0]+min[0])/2.0f, -(max[1]+min[1])/2.0f, -min[2]), Eigen::Quaternion<float>(0,0,0,0));
260  }
261 
262  pcl::io::savePCDFile(uFormat("extracted_object%d.pcd", (int)i+1), *object);
263  printf("Saved extracted_object%d.pcd (%d points)\n", (int)i+1, (int)object->size());
264  }
265 
266  return 0;
267 }
d
d
showUsage
void showUsage()
Definition: tools/ExtractObject/main.cpp:40
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
rtabmap::util3d::getMinMax3D
void RTABMAP_EXP getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2666
f
f
uStr2Float
float UTILITE_EXP uStr2Float(const std::string &str)
Definition: UConversion.cpp:123
UConversion.h
Some conversion functions.
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap_oanet.argv
argv
Definition: rtabmap_oanet.py:12
main
int main(int argc, char *argv[])
Definition: tools/ExtractObject/main.cpp:52
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59