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 }
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
showUsage
void showUsage()
Definition: tools/ExtractObject/main.cpp:40
b
Array< int, 3, 1 > b
c
Scalar Scalar * c
rtabmap_netvlad.extract
def extract(image)
Definition: rtabmap_netvlad.py:53
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
q
EIGEN_DEVICE_FUNC const Scalar & q
UConversion.h
Some conversion functions.
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
object
target
target
d
d
rtabmap_netvlad.argv
argv
Definition: rtabmap_netvlad.py:15
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
Eigen::Quaternion< float >
a
ArrayXXi a
trans
Matrix trans(const Matrix &A)
rtabmap::util3d::getMinMax3D
void RTABMAP_CORE_EXPORT getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2613
main
int main(int argc, char *argv[])
Definition: tools/ExtractObject/main.cpp:52
uStr2Float
float UTILITE_EXPORT uStr2Float(const std::string &str)
Definition: UConversion.cpp:138
i
int i

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:12