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
00036 #ifndef _polygon_primitive_operations_CPP_
00037 #define _polygon_primitive_operations_CPP_
00038
00039
00040 #include "polygon_primitive.h"
00041
00054 int c_polygon_primitive::polygon_create(pcl::PointCloud<pcl::PointXYZ> *input_cloud,
00055 pcl::PointCloud<pcl::Normal> *input_normals,
00056 double DistanceThreshold,
00057 double NormalDistanceWeight,
00058 int MaxIterations,
00059 int do_spatial_division)
00060 {
00061
00062 ros::Time start_tic = ros::Time::now();
00063 data.hulls.convex.area = 0;
00064 data.hulls.convex.solidity = 0;
00065 int polygon_created_correctly=true;
00066
00067
00068
00069
00070 data.frames.global_name = input_cloud->header.frame_id;
00071 set_reference_systems();
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081 int j=0, max_cluster=0, count=0;
00082 int num_clusters=0;
00083 pcl::PointIndices::Ptr indices = pcl::PointIndices::Ptr(new pcl::PointIndices);
00084
00085
00086
00087
00088
00089
00090 data.frames.global_name = input_cloud->header.frame_id;
00091 pointclouds.all->header.frame_id = data.frames.global_name;
00092 data.hulls.convex.polygon->header.frame_id = data.frames.global_name;
00093 pointclouds.additional->header.frame_id = data.frames.global_name;
00094
00095
00096
00097
00098
00099
00100 if (do_spatial_division==0)
00101 {
00102
00103
00104
00105 if (!compute_supporting_plane_ransac( input_cloud,
00106 input_normals,
00107 DistanceThreshold,
00108 NormalDistanceWeight,
00109 MaxIterations,
00110 indices,
00111 data.planes.current
00112 ))
00113 {
00114 ROS_WARN("polygon create: cannot create polygon"); return 0;
00115 polygon_created_correctly=false;
00116 }
00117
00118
00119
00120 }
00121 else
00122 {
00123
00124
00125
00126 if (!compute_supporting_perpendicular_plane_ransac( input_cloud,
00127 input_normals,
00128 DistanceThreshold,
00129 NormalDistanceWeight,
00130 MaxIterations,
00131 indices,
00132 data.planes.current
00133 ))
00134 {
00135 ROS_WARN("PARALELL polygon create: cannot create polygon"); return 0;
00136 polygon_created_correctly=false;
00137 }
00138
00139
00140 }
00141
00142
00143
00144
00145
00146 indices_extraction(indices,
00147 input_cloud,
00148 input_cloud,
00149 pointclouds.all,
00150 input_normals,
00151 input_normals);
00152
00153
00154
00155
00156
00157
00158
00159
00160 if (do_spatial_division==1 && polygon_created_correctly==true)
00161 {
00162
00163
00164
00165 j=0; max_cluster=0; count=0; num_clusters=0;
00166
00167
00168
00169
00170 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1 (new pcl::search::KdTree<pcl::PointXYZ>);
00171
00172
00173
00174
00175
00176 pcl::PointCloud<pcl::PointXYZ> cluster;
00177 tree1->setInputCloud(pointclouds.all->makeShared());
00178
00179 std::vector<pcl::PointIndices> cluster_indices;
00180 pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
00181 ec.setClusterTolerance(0.9);
00182 ec.setMinClusterSize(1);
00183 ec.setMaxClusterSize((unsigned int)pointclouds.all->size());
00184 ec.setSearchMethod(tree1);
00185 ec.setInputCloud(pointclouds.all->makeShared());
00186 ec.extract(cluster_indices);
00187
00188
00189
00190
00191 for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
00192 {
00193 int t=0;
00194 for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
00195 {
00196 t++;
00197 }
00198
00199 if(t>count)
00200 {
00201 max_cluster = j;
00202 count = t;
00203 }
00204 j++;
00205 }
00206
00207
00208
00209 j=0;
00210 pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud;
00211 tmp_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
00212
00213
00214 for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
00215 {
00216 if (j==max_cluster)
00217 {
00218 tmp_cloud->width=0;
00219 for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
00220 {
00221 tmp_cloud->width++;
00222 tmp_cloud->points.push_back(pointclouds.all->points[*pit]);
00223 }
00224 }
00225 else
00226 {
00227
00228
00229
00230
00231
00232 }
00233 j++;
00234 }
00235 num_clusters=j;
00236
00237
00238 (*pointclouds.all) = (*tmp_cloud);
00239
00240
00241
00242
00243
00244
00245 double fit_qual = fit_plane_to_pc(pointclouds.all, data.planes.current);
00246 fit_qual=fit_qual;
00247
00248
00249
00250
00251
00252
00253 double fit_qual2 = fit_plane_to_two_pc_and_ratio(pointclouds.all, pointclouds.all, 0.5, data.planes.current);
00254 fit_qual2=fit_qual2;
00255
00256
00257 }
00258
00259
00260
00261 if (polygon_created_correctly==true)
00262 {
00263 check_plane_normal_orientation(data.planes.current, &pointclouds.all->points[0], 100000, 100000, 100000);
00264
00265
00266
00267
00268
00269
00270 create_reference_frame_from_plane_and_two_points(data.planes.current,
00271 &pointclouds.all->points[0],
00272 &pointclouds.all->points[1],
00273 &data.frames.current);
00274
00275 compute_convex_hull_cgal(pointclouds.all, data.planes.current, data.hulls.convex.polygon, &data.frames.current.transform, data.hulls.convex.area, data.hulls.convex.solidity,false);
00276 pointclouds.projected->header.frame_id = "/world";
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00293
00294
00295
00296
00297 offset_polygon(.1, data.hulls.convex.polygon, data.hulls.convex.extended_polygon, &data.frames.current.transform);
00298
00299
00300 copy(&data.frames.current, &data.frames.previous);
00301 }
00302
00303
00304
00305 ros::Duration time1 = ros::Time::now() - start_tic;
00306 #if 0
00307
00308 ROS_INFO("%s create in %3.2f seconds\n \
00309 Creation %s\n \
00310 Color rgb [%d,%d,%d]\n \
00311 Initial point cloud with %d points, after with %d points\n \
00312 Using Euclidean cluster extraction: %s\n \
00313 Divided into %d clusters. Largest %d with %d points\n \
00314 Support plane is A=%3.2f B=%3.2f C=%3.2f D=%3.2f \n \
00315 Number of support points %d \n \
00316 Convex Hull with %d points, Area of %3.2f and Solidity of %3.2f \n \
00317 Concave Hull with %d points, Area of %3.2f and Solidity of %3.2f \n \
00318 ", data.misc.name, time1.toSec(), (polygon_created_correctly==true)?"successfully":"failure. No ransac candidate output",data.misc.color.r, data.misc.color.g, data.misc.color.b, (int)num_start_points, (int)input_cloud->size(), (do_spatial_division==true)?"yes":"no", num_clusters, max_cluster, count, data.planes.current->values[0], data.planes.current->values[1],data.planes.current->values[2],data.planes.current->values[3],(int)pointclouds.all->size(), (int)data.hulls.convex.polygon->points.size(),data.hulls.convex.area, data.hulls.convex.solidity,(int) data.hulls.concave.polygon->points.size(),0.,0.);
00319
00320
00321 #endif
00322
00323
00324 indices.reset();
00325
00326 if (polygon_created_correctly==true)
00327 return 1;
00328 else
00329 return 0;
00330 }
00331
00341 int c_polygon_primitive::polygon_expansion(pcl::PointCloud<pcl::PointXYZ> *input_cloud,
00342 double longitudinal_offset,
00343 double perpendicular_offset,
00344 pcl::PointCloud<pcl::Normal>* input_normals)
00345 {
00346 ros::Time start_tic = ros::Time::now();
00347 int num_support_points_before_expansion = (int)pointclouds.all->points.size();
00348 bool significative_expansion=false;
00349 int old_convex_hull_num_pts = (int)data.hulls.convex.polygon->points.size();
00350 double old_convex_hull_area = data.hulls.convex.area;
00351 double old_convex_hull_solidity = data.hulls.convex.solidity;
00352
00353
00354 pointclouds.growed->points.erase(pointclouds.growed->points.begin(),
00355 pointclouds.growed->points.end());
00356
00357 int num_expansions=0;
00358 int flag_keep_expanding=1;
00359
00360
00361
00362 ros::Time t = ros::Time::now();
00363 while (flag_keep_expanding)
00364 {
00365 flag_keep_expanding = _polygon_expansion(input_cloud, longitudinal_offset, perpendicular_offset, input_normals);
00366 num_expansions++;
00367 }
00368
00369 if ((int)pointclouds.growed->points.size()==0)
00370 {
00371 significative_expansion=false;
00372 }
00373 else
00374 {
00375 significative_expansion=true;
00376
00377
00378
00379
00380 copy(&data.frames.current, &data.frames.previous);
00381 copy(data.planes.current, data.planes.previous);
00382
00383
00384 double ratio = (double)pointclouds.growed->points.size() /
00385 ((double)pointclouds.growed->points.size() +(double)pointclouds.all->points.size());
00386
00387 double fit_qual3 = fit_plane_to_two_pc_and_ratio(pointclouds.all, pointclouds.growed, ratio, data.planes.current);
00388 fit_qual3=fit_qual3;
00389
00390 project_pc_to_plane(pointclouds.all, data.planes.current, pointclouds.projected);
00391
00392 create_reference_frame_from_plane_and_two_points(data.planes.current,
00393 &pointclouds.all->points[0],
00394 &pointclouds.all->points[1],
00395 &data.frames.current);
00396
00397
00398
00399
00400 compute_convex_hull_cgal(data.hulls.convex.polygon,data.planes.current, data.hulls.convex.polygon, &data.frames.current.transform);
00401
00402
00403 pointclouds.growed->header.frame_id = pointclouds.all->header.frame_id;
00404 (*pointclouds.all) += (*pointclouds.growed);
00405
00406
00407
00408
00409 }
00410
00411 ros::Duration time1 = ros::Time::now() - start_tic;
00412
00413 #if 1
00414
00415 ROS_INFO("%s Expansion in %3.2f seconds\n \
00416 Expansion point cloud with %d points, after with %d points\n \
00417 Expansion params: longitudinal offset=%3.2f perpendicular offset=%3.2f \n \
00418 Polygon expanded %d times to a total of %d points \n \
00419 Number of support points at T-1 %d, at T %d \n \
00420 Support plane was recomputed: %s \n \
00421 Support plane at T-1 A=%3.2f B=%3.2f C=%3.2f D=%3.2f \n \
00422 Support plane at T A=%3.2f B=%3.2f C=%3.2f D=%3.2f \n \
00423 Convex Hull at T-1 with %d points, Area of %3.2f and Solidity of %3.2f \n \
00424 Convex Hull at T with %d points, Area of %3.2f and Solidity of %3.2f \n \
00425 Concave Hull with %d points, Area of %3.2f and Solidity of %3.2f \n \
00426 ", data.misc.name, time1.toSec(), (int)num_support_points_before_expansion, (int)pointclouds.all->points.size(), longitudinal_offset, perpendicular_offset, num_expansions, (int)pointclouds.growed->points.size(), num_support_points_before_expansion, (int)pointclouds.all->size(), significative_expansion?"yes":"no", data.planes.previous->values[0], data.planes.previous->values[1],data.planes.previous->values[2],data.planes.previous->values[3],data.planes.current->values[0], data.planes.current->values[1],data.planes.current->values[2],data.planes.current->values[3],old_convex_hull_num_pts, old_convex_hull_area, old_convex_hull_solidity, (int)data.hulls.convex.polygon->points.size(),data.hulls.convex.area, data.hulls.convex.solidity,(int) data.hulls.concave.polygon->points.size(),0.,0.);
00427
00428
00429 #endif
00430 return 1;}
00431
00441 int c_polygon_primitive::_polygon_expansion(pcl::PointCloud<pcl::PointXYZ> *input_cloud,
00442 double longitudinal_offset,
00443 double perpendicular_offset,
00444 pcl::PointCloud<pcl::Normal>* input_normals)
00445 {
00446
00447
00448
00449 offset_polygon(longitudinal_offset, data.hulls.convex.polygon, data.hulls.convex.extended_polygon, &data.frames.current.transform);
00450
00451
00452 pcl::ExtractPolygonalPrismData<pcl::PointXYZ> epp;
00453 pcl::PointCloud<pcl::PointXYZ>::ConstPtr input_cloud_constptr;
00454 input_cloud_constptr.reset (new pcl::PointCloud<pcl::PointXYZ> (*input_cloud));
00455 pcl::PointCloud<pcl::PointXYZ>::ConstPtr convex_hull_constptr;
00456 convex_hull_constptr.reset (new pcl::PointCloud<pcl::PointXYZ> (*data.hulls.convex.extended_polygon));
00457 pcl::PointIndices::Ptr ind = pcl::PointIndices::Ptr(new pcl::PointIndices);
00458 pcl::ExtractIndices<pcl::PointXYZ> extract;
00459 pcl::PointIndices::Ptr indices;
00460 indices.reset();
00461 indices = pcl::PointIndices::Ptr(new pcl::PointIndices);
00462
00463
00464 epp.setInputCloud(input_cloud_constptr);
00465 epp.setInputPlanarHull(convex_hull_constptr);
00466 epp.setHeightLimits(-perpendicular_offset, perpendicular_offset);
00467 epp.setViewPoint(10000,10000,10000);
00468
00469
00470 epp.segment(*ind);
00471
00472 indices_extraction(ind,
00473 input_cloud,
00474 input_cloud,
00475 pointclouds.additional,
00476 input_normals,
00477 input_normals);
00478
00479
00480
00481
00482
00483 pointclouds.growed->header.frame_id = data.frames.global_name;
00484 pointclouds.all->header.frame_id = data.frames.global_name;
00485 (*pointclouds.growed) += (*pointclouds.additional);
00486
00487
00488 if ((int)pointclouds.additional->size() == 0)
00489 {
00490 indices.reset();
00491 return 0;
00492 }
00493
00494
00495
00496
00497
00498
00499 pointclouds.tmp->header.frame_id = data.frames.global_name;
00500 data.hulls.convex.polygon->header.frame_id = data.frames.global_name;
00501 pointclouds.growed->header.frame_id = data.frames.global_name;
00502
00503 (*pointclouds.tmp) = (*data.hulls.convex.polygon);
00504
00505 (*pointclouds.tmp) += (*pointclouds.growed);
00506
00507
00508
00509
00510
00511 compute_convex_hull_cgal(pointclouds.tmp,data.planes.current, data.hulls.convex.polygon, &data.frames.current.transform);
00512
00513 return 1;
00514 }
00515
00516 void c_polygon_primitive::polygon_split(pcl::PointCloud<pcl::PointXYZ>::Ptr pcin, pcl::PointCloud<pcl::PointXYZ>::Ptr pcdumpster, pcl::ModelCoefficients::Ptr coeff)
00517 {
00518
00519 int j=0, max_cluster=0, count=0, num_clusters=0;
00520
00521
00522
00523
00524
00525 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
00526 pcl::PointCloud<pcl::PointXYZ> cluster;
00527 tree->setInputCloud(pcin->makeShared());
00528
00529 std::vector<pcl::PointIndices> cluster_indices;
00530 pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
00531 ec.setClusterTolerance(1.0);
00532 ec.setMinClusterSize(1);
00533 ec.setMaxClusterSize((unsigned int)pcin->size());
00534 ec.setSearchMethod(tree);
00535 ec.setInputCloud(pcin->makeShared());
00536 ec.extract(cluster_indices);
00537
00538
00539 for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
00540 {
00541 int t=0;
00542 for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
00543 {
00544 t++;
00545 }
00546
00547 if(t>count)
00548 {
00549 max_cluster = j;
00550 count = t;
00551 }
00552 j++;
00553 }
00554
00555
00556 j=0;
00557 pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
00558
00559 for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
00560 {
00561 if (j==max_cluster)
00562 {
00563 tmp_cloud->width=0;
00564 for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
00565 {
00566 tmp_cloud->width++;
00567 tmp_cloud->points.push_back(pcin->points[*pit]);
00568 }
00569 }
00570 else
00571 {
00572 for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
00573 {
00574 pcdumpster->width++;
00575 pcdumpster->points.push_back(pcin->points[*pit]);
00576 }
00577 }
00578 j++;
00579 }
00580 num_clusters=j;
00581
00582 (*pcin) = (*tmp_cloud);
00583 tmp_cloud.reset();
00584
00585 refine_plane_coefficients(pcin, coeff);
00586
00587 ROS_INFO("Performing Euclidian cluster extraction optimization:");
00588 ROS_INFO("Largest cluster is %d with %d points (%d clusters)",max_cluster,count, num_clusters);
00589 ROS_INFO("New support plane A=%3.2f B=%3.2f C=%3.2f D=%3.2f supported by %d points",data.planes.current->values[0], data.planes.current->values[1],data.planes.current->values[2],data.planes.current->values[3],(int)pointclouds.all->size());
00590
00591 }
00592
00593
00594 #endif
00595
00598