00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 #include "ros/ros.h"
00034
00035
00036 #include "terminal_tools/parse.h"
00037
00038
00039 #include "pcl/io/pcd_io.h"
00040 #include "pcl/features/normal_3d.h"
00041 #include "pcl/surface/convex_hull.h"
00042 #include "pcl/filters/extract_indices.h"
00043 #include "pcl/filters/project_inliers.h"
00044 #include "pcl/sample_consensus/method_types.h"
00045 #include "pcl/segmentation/sac_segmentation.h"
00046 #include "pcl/segmentation/extract_clusters.h"
00047 #include "pcl/segmentation/extract_polygonal_prism_data.h"
00048
00049
00050 #include "pcl_visualization/pcl_visualizer.h"
00051
00052
00053
00054
00055 typedef pcl::PointXYZ PointT;
00056
00057
00058
00059
00060 double epsilon_angle = 0.010;
00061 double plane_threshold = 0.100;
00062 int minimum_plane_inliers = 1000;
00063 int maximum_plane_iterations = 1000;
00064
00065
00066 bool step = false;
00067 bool clean = false;
00068 bool verbose = false;
00069 int size_of_points = 1;
00070
00071
00072
00074
00076 int main (int argc, char** argv)
00077 {
00078
00079
00080
00081
00082
00083
00084 if (argc < 2)
00085 {
00086 ROS_INFO (" ");
00087 ROS_INFO ("Syntax is: %s <input>.pcd <options>", argv[0]);
00088 ROS_INFO (" where <options> are: ");
00089 ROS_INFO (" -epsilon_angle X = ");
00090 ROS_INFO (" -plane_threshold X = ");
00091 ROS_INFO (" -minimum_plane_inliers X = ");
00092 ROS_INFO (" -maximum_plane_iterations X = ");
00093 ROS_INFO (" ");
00094 ROS_INFO (" -step B = wait or not wait");
00095 ROS_INFO (" -clean B = remove or not remove");
00096 ROS_INFO (" -verbose B = display step by step info");
00097 ROS_INFO (" -size_of_points D = set the size of points");
00098 ROS_INFO (" ");
00099 return (-1);
00100 }
00101
00102
00103 std::vector<int> pFileIndicesPCD = terminal_tools::parse_file_extension_argument (argc, argv, ".pcd");
00104 if (pFileIndicesPCD.size () == 0)
00105 {
00106 ROS_ERROR ("No .pcd file given as input!");
00107 return (-1);
00108 }
00109
00110
00111 terminal_tools::parse_argument (argc, argv, "-epsilon_angle", epsilon_angle);
00112 terminal_tools::parse_argument (argc, argv, "-plane_threshold", plane_threshold);
00113 terminal_tools::parse_argument (argc, argv, "-minimum_plane_inliers", minimum_plane_inliers);
00114 terminal_tools::parse_argument (argc, argv, "-maximum_plane_iterations", maximum_plane_iterations);
00115
00116
00117 terminal_tools::parse_argument (argc, argv, "-step", step);
00118 terminal_tools::parse_argument (argc, argv, "-clean", clean);
00119 terminal_tools::parse_argument (argc, argv, "-verbose", verbose);
00120 terminal_tools::parse_argument (argc, argv, "-size_of_points", size_of_points);
00121
00122
00123
00124
00125
00126
00127 srand (time(0));
00128
00129
00130 ros::Time::init();
00131
00132
00133 terminal_tools::TicToc tt;
00134
00135
00136 tt.tic ();
00137
00138 if ( verbose )
00139 {
00140
00141 ROS_WARN ("Timer started !");
00142 }
00143
00144
00145
00146
00147
00148
00149 pcl_visualization::PCLVisualizer viewer ("3D VIEWER");
00150
00151 viewer.setBackgroundColor (1.0, 1.0, 1.0);
00152
00153 viewer.addCoordinateSystem (0.75f);
00154
00155 viewer.getCameraParameters (argc, argv);
00156
00157 viewer.updateCamera ();
00158
00159
00160
00161
00162
00163
00164 pcl::PointCloud<PointT>::Ptr input_cloud (new pcl::PointCloud<PointT> ());
00165
00166
00167 if (pcl::io::loadPCDFile (argv[pFileIndicesPCD[0]], *input_cloud) == -1)
00168 {
00169 ROS_ERROR ("Couldn't read file %s", argv[pFileIndicesPCD[0]]);
00170 return (-1);
00171 }
00172 ROS_INFO ("Loaded %d data points from %s with the following fields: %s", (int) (input_cloud->points.size ()), argv[pFileIndicesPCD[0]], pcl::getFieldsList (*input_cloud).c_str ());
00173
00174
00175 viewer.addPointCloud (*input_cloud, "INPUT");
00176
00177
00178 viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 0.0, "INPUT");
00179
00180
00181 viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points - 1, "INPUT");
00182
00183
00184 if ( step )
00185 {
00186
00187 viewer.spin ();
00188 }
00189
00190 if ( clean )
00191 {
00192
00193 viewer.removePointCloud ("INPUT");
00194
00195
00196 if ( step )
00197 {
00198
00199 viewer.spin ();
00200 }
00201 }
00202
00203
00204
00205
00206
00207
00208 pcl::PointCloud<PointT>::Ptr working_cloud (new pcl::PointCloud<PointT> ());
00209
00210
00211 *working_cloud = *input_cloud;
00212
00213
00214
00215
00216
00217
00218 pcl::PointCloud<pcl::Normal>::Ptr normals_cloud (new pcl::PointCloud<pcl::Normal> ());
00219
00220 pcl::KdTreeFLANN<PointT>::Ptr normals_tree (new pcl::KdTreeFLANN<PointT> ());
00221
00222
00223 pcl::NormalEstimation<PointT, pcl::Normal> estimation_of_normals;
00224
00225 estimation_of_normals.setSearchMethod (normals_tree);
00226
00227 estimation_of_normals.setInputCloud (working_cloud);
00228
00229 estimation_of_normals.setKSearch (50);
00230
00231 estimation_of_normals.compute (*normals_cloud);
00232
00233 if ( verbose )
00234 {
00235 ROS_INFO ("Remaning cloud has %d points", (int) working_cloud->points.size());
00236 ROS_INFO ("With %d normals of course", (int) normals_cloud->points.size());
00237 }
00238
00239
00240
00241
00242
00243
00244 pcl::SACSegmentationFromNormals<PointT, pcl::Normal> segmentation_of_plane;
00245 pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices ());
00246 pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients ());
00247
00248
00249 segmentation_of_plane.setOptimizeCoefficients (true);
00250 segmentation_of_plane.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00251 segmentation_of_plane.setNormalDistanceWeight (0.05);
00252 segmentation_of_plane.setMethodType (pcl::SAC_RANSAC);
00253 segmentation_of_plane.setDistanceThreshold (plane_threshold);
00254 segmentation_of_plane.setMaxIterations (maximum_plane_iterations);
00255
00256
00257
00258 segmentation_of_plane.setEpsAngle (epsilon_angle);
00259 segmentation_of_plane.setInputCloud (working_cloud);
00260 segmentation_of_plane.setInputNormals (normals_cloud);
00261
00262
00263 segmentation_of_plane.segment (*plane_inliers, *plane_coefficients);
00264
00265 if ( verbose )
00266 {
00267 ROS_INFO ("Plane has %5d inliers with parameters A = %f B = %f C = %f and D = %f found in maximum %d iterations", (int) plane_inliers->indices.size (),
00268 plane_coefficients->values [0], plane_coefficients->values [1], plane_coefficients->values [2], plane_coefficients->values [3], maximum_plane_iterations);
00269 }
00270
00271
00272 if ((int) plane_inliers->indices.size () < minimum_plane_inliers)
00273 {
00274 if ( verbose )
00275 {
00276 ROS_ERROR ("NOT ACCEPTED !");
00277 }
00278 }
00279 else
00280 {
00281 if ( verbose )
00282 {
00283 ROS_WARN ("ACCEPTED !");
00284 }
00285
00286
00287
00288
00289 pcl::PointCloud<PointT>::Ptr plane_cloud (new pcl::PointCloud<PointT> ());
00290
00291
00292 pcl::ExtractIndices<PointT> extraction_of_inliers;
00293 extraction_of_inliers.setInputCloud (working_cloud);
00294 extraction_of_inliers.setIndices (plane_inliers);
00295 extraction_of_inliers.setNegative (false);
00296 extraction_of_inliers.filter (*plane_cloud);
00297 extraction_of_inliers.setNegative (true);
00298 extraction_of_inliers.filter (*working_cloud);
00299
00300
00301 pcl::ExtractIndices<pcl::Normal> extraction_of_normals;
00302 extraction_of_normals.setInputCloud (normals_cloud);
00303 extraction_of_normals.setIndices (plane_inliers);
00304 extraction_of_normals.setNegative (true);
00305 extraction_of_normals.filter (*normals_cloud);
00306
00307 if ( verbose )
00308 {
00309 ROS_INFO ("Remaning cloud has %d points", (int) working_cloud->points.size());
00310 ROS_INFO ("With %d normals of course", (int) normals_cloud->points.size());
00311 }
00312
00313
00314 std::stringstream id_of_plane;
00315 id_of_plane << "PLANE_" << ros::Time::now ();
00316
00317
00318 viewer.addPointCloud (*plane_cloud, id_of_plane.str());
00319
00320
00321 viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points, id_of_plane.str());
00322
00323
00324 if ( step )
00325 {
00326
00327 viewer.spin ();
00328 }
00329
00330 if ( clean )
00331 {
00332
00333 viewer.removePointCloud (id_of_plane.str());
00334
00335
00336 if ( step )
00337 {
00338
00339 viewer.spin ();
00340 }
00341 }
00342
00343
00344 std::string file_of_plane = argv [pFileIndicesPCD[0]];
00345 size_t dot_of_plane = file_of_plane.find (".");
00346 file_of_plane.insert (dot_of_plane, "-plane");
00347
00348
00349 pcl::io::savePCDFile (file_of_plane, *plane_cloud);
00350
00351
00352
00353
00354 pcl::PointCloud<PointT> projected_cloud;
00355
00356
00357 pcl::ProjectInliers<PointT> projection_on_plane;
00358 projection_on_plane.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00359 projection_on_plane.setInputCloud (plane_cloud);
00360 projection_on_plane.setModelCoefficients (plane_coefficients);
00361 projection_on_plane.filter (projected_cloud);
00362
00363 if ( verbose )
00364 {
00365 ROS_INFO ("Furniture surface has %d points", (int) projected_cloud.points.size ());
00366 }
00367
00368
00369 std::stringstream id_of_projected;
00370 id_of_projected << "PROJECTED_" << ros::Time::now ();
00371
00372
00373 viewer.addPointCloud (projected_cloud, id_of_projected.str());
00374
00375
00376 viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points, id_of_projected.str());
00377
00378
00379 if ( step )
00380 {
00381
00382 viewer.spin ();
00383 }
00384
00385
00386 std::string file_of_projected = argv [pFileIndicesPCD[0]];
00387 size_t dot_of_projected = file_of_projected.find (".");
00388 file_of_projected.insert (dot_of_projected, "-projected");
00389
00390
00391 pcl::io::savePCDFile (file_of_projected, projected_cloud);
00392
00393
00394
00395
00396 pcl::PointCloud<PointT> hull_cloud;
00397
00398
00399 pcl::ConvexHull<PointT> hull;
00400 hull.setInputCloud (projected_cloud.makeShared ());
00401 ROS_ERROR (" BEFORE IT WORKS ");
00402 hull.reconstruct (hull_cloud);
00403 ROS_ERROR (" AFTERWARDS NOT ");
00404
00405 if ( verbose )
00406 {
00407 ROS_INFO ("Convex hull has %d points.", (int) hull_cloud.points.size ());
00408 }
00409
00410
00411 std::stringstream id_of_hull;
00412 id_of_hull << "HULL_" << ros::Time::now ();
00413
00414
00415 viewer.addPointCloud (hull_cloud, id_of_hull.str());
00416
00417
00418 viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points, id_of_hull.str());
00419
00420
00421 if ( step )
00422 {
00423
00424 viewer.spin ();
00425 }
00426
00427 if ( clean )
00428 {
00429
00430 viewer.removePointCloud (id_of_hull.str());
00431
00432
00433 if ( step )
00434 {
00435
00436 viewer.spin ();
00437 }
00438 }
00439
00440
00441 std::string file_of_hull = argv [pFileIndicesPCD[0]];
00442 size_t dot_of_hull = file_of_hull.find (".");
00443 file_of_hull.insert (dot_of_hull, "-hull");
00444
00445
00446 pcl::io::savePCDFile (file_of_hull, hull_cloud);
00447
00448
00449
00450
00451 pcl::PointIndices::Ptr handle_indices (new pcl::PointIndices ());
00452
00453
00454 pcl::ExtractPolygonalPrismData<PointT> prism;
00455 prism.setHeightLimits (0.025, 0.100);
00456 prism.setInputCloud (input_cloud);
00457 prism.setInputPlanarHull (hull_cloud.makeShared());
00458 prism.setViewPoint (0.0, 0.0, 1.5);
00459 prism.segment (*handle_indices);
00460
00461 if ( verbose )
00462 {
00463 ROS_INFO ("The number of handle indices %d", (int) handle_indices->indices.size ());
00464 }
00465
00466
00467 pcl::PointCloud<PointT> handle_cloud;
00468
00469
00470 pcl::ExtractIndices<PointT> extraction_of_handle;
00471 extraction_of_handle.setInputCloud (input_cloud);
00472 extraction_of_handle.setIndices (handle_indices);
00473 extraction_of_handle.setNegative (false);
00474 extraction_of_handle.filter (handle_cloud);
00475
00476
00477 std::stringstream id_of_handle;
00478 id_of_handle << "HANDLE_" << ros::Time::now ();
00479
00480
00481 viewer.addPointCloud (handle_cloud, id_of_handle.str());
00482
00483
00484 viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points, id_of_handle.str());
00485
00486
00487 if ( step )
00488 {
00489
00490 viewer.spin ();
00491 }
00492
00493
00494 std::string file_of_handle = argv [pFileIndicesPCD[0]];
00495 size_t dot_of_handle = file_of_handle.find (".");
00496 file_of_handle.insert (dot_of_handle, "-handle");
00497
00498
00499 pcl::io::savePCDFile (file_of_handle, handle_cloud);
00500
00501
00502
00503 }
00504
00505 if ( verbose )
00506 {
00507
00508 ROS_WARN ("Finished in %5.3g [s] !", tt.toc ());
00509 }
00510
00511
00512 viewer.spin ();
00513
00514 return (0);
00515 }