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