polygon_primitive_auxiliary.cpp
Go to the documentation of this file.
00001 /**************************************************************************************************
00002  Software License Agreement (BSD License)
00003 
00004  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
00005  All rights reserved.
00006 
00007  Redistribution and use in source and binary forms, with or without modification, are permitted
00008  provided that the following conditions are met:
00009 
00010   *Redistributions of source code must retain the above copyright notice, this list of
00011    conditions and the following disclaimer.
00012   *Redistributions in binary form must reproduce the above copyright notice, this list of
00013    conditions and the following disclaimer in the documentation and/or other materials provided
00014    with the distribution.
00015   *Neither the name of the University of Aveiro nor the names of its contributors may be used to
00016    endorse or promote products derived from this software without specific prior written permission.
00017  
00018  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
00019  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00020  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00021  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00022  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00023  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
00024  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
00025  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 ***************************************************************************************************/
00036 #ifndef _polygon_primitive_auxiliary_CPP_
00037 #define _polygon_primitive_auxiliary_CPP_
00038 
00039 #include "polygon_primitive.h"
00040 
00041 
00042 int c_polygon_primitive::compute_supporting_perpendicular_plane_ransac( pcl::PointCloud<pcl::PointXYZ> *pc_in,
00043                 pcl::PointCloud<pcl::Normal> *n_in,
00044                 double DistanceThreshold,
00045                 double NormalDistanceWeight,
00046                 int MaxIterations, 
00047                 pcl::PointIndices::Ptr ind_out,
00048                 pcl::ModelCoefficients::Ptr coeff_out
00049                 )
00050 {
00051         // Create the segmentation objects
00052         pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg_normals; 
00053         //Setup segmentation parameters
00054         seg_normals.setOptimizeCoefficients(true);
00055         seg_normals.setModelType(pcl::SACMODEL_NORMAL_PARALLEL_PLANE);
00056         //seg_normals.setModelType(pcl::SACMODEL_NORMAL_PERPENDICULAR_PLANE);
00057         seg_normals.setMethodType (pcl::SAC_RANSAC); 
00058         seg_normals.setMaxIterations(MaxIterations+0);
00059         seg_normals.setNormalDistanceWeight(NormalDistanceWeight);
00060         seg_normals.setDistanceThreshold(DistanceThreshold);
00061         seg_normals.setInputCloud(pc_in->makeShared());
00062         seg_normals.setInputNormals(n_in->makeShared());
00063         Eigen::Vector3f v;                                                                
00064         v[0] = 0; v[1] = 0; v[2] = 1;
00065         seg_normals.setAxis(v);
00066         seg_normals.setEpsAngle(M_PI*30.0 / 180.0);
00067         //seg_normals.setDistanceFromOrigin(0);
00068         //seg_normals.setEpsDist(1000);
00069 
00070         seg_normals.segment(*ind_out, *coeff_out);
00071         //pcl::SampleConsensusModelNormalParallelPlane<pcl::PointXYZ, pcl::Normal> model(pc_in->makeShared(), indices); 
00072 
00073         //model.setInputCloud(pc_in->makeShared());
00074         //model.setInputNormals(n_in->makeShared());
00075         //model.setNormalDistanceWeight(NormalDistanceWeight);
00076 
00077         //Eigen::Vector3f v;                                                                
00078         //v[0] = 0; v[1] = 0; v[2] = 1;
00079         //model.setAxis(v);
00080 
00081         //model.setEpsAngle(M_PI*15/180.0);
00082 
00083 
00084 
00085         //model.setDistanceFromOrigin(0);
00086         //model.setEpsDist(1000);
00087 
00088 
00089         //model.setIndices(indices);
00090 
00091         //Eigen::VectorXf mc; 
00092         //std::vector<int> indices_out;
00093         //printf("computing\n");
00094 
00096         //printf("computing done\n");
00097 
00098         //SampleConsensusModelNormalParallelPlane
00099         //Setup segmentation parameters
00100         //seg_normals.setOptimizeCoefficients(true);
00101         //seg_normals.setModelType(pcl::SACMODEL_NORMAL_PARALLEL_PLANE);
00102         //seg_normals.setMethodType (pcl::SAC_RANSAC); 
00103         //seg_normals.setMaxIterations(MaxIterations);
00104         //seg_normals.setNormalDistanceWeight(NormalDistanceWeight);
00105         //seg_normals.setDistanceThreshold(DistanceThreshold);
00106         //seg_normals.setDistanceThreshold(DistanceThreshold);
00107 
00109         //seg_normals.setInputCloud(pc_in->makeShared());
00110         //seg_normals.setInputNormals(n_in->makeShared());
00111         //seg_normals.segment(*ind_out, *coeff_out);
00112 
00113 
00114         //ROS_INFO("NORMAL PARALLEL PLANE computed supporting planee A=%3.2f B=%3.2f C=%3.2f D=%3.2f supported by %d points",mc[0],  mc[1],mc[2], mc[3],(int)pc_in->size());   
00115 
00116         if (ind_out->indices.size() < 10)
00117         {
00118                 //ROS_WARN("NORMAL PARALLEL PLANE Could not estimate a planar model for the given dataset. Indices size = %ld", ind_out->indices.size());
00119                 return 0;
00120         }
00121 
00122 
00123 
00124         return 1;}
00125 
00126 
00127         int c_polygon_primitive::compute_supporting_plane_ransac( pcl::PointCloud<pcl::PointXYZ> *pc_in,
00128                         pcl::PointCloud<pcl::Normal> *n_in,
00129                         double DistanceThreshold,
00130                         double NormalDistanceWeight,
00131                         int MaxIterations, 
00132                         pcl::PointIndices::Ptr ind_out,
00133                         pcl::ModelCoefficients::Ptr coeff_out
00134                         )
00135 {
00136         // Create the segmentation objects
00137         pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg_normals; 
00138         //Setup segmentation parameters
00139         seg_normals.setOptimizeCoefficients(true);
00140         seg_normals.setModelType(pcl::SACMODEL_NORMAL_PLANE);
00141         seg_normals.setMethodType (pcl::SAC_RANSAC); 
00142         seg_normals.setMaxIterations(MaxIterations);
00143         seg_normals.setNormalDistanceWeight(NormalDistanceWeight);
00144         seg_normals.setDistanceThreshold(DistanceThreshold);
00145 
00146         //Compute  a plane candidate from the point cloud
00147         seg_normals.setInputCloud(pc_in->makeShared());
00148         seg_normals.setInputNormals(n_in->makeShared());
00149         seg_normals.segment(*ind_out, *coeff_out);
00150 
00151 
00152         //ROS_INFO("computed supporting planee A=%3.2f B=%3.2f C=%3.2f D=%3.2f supported by %d points",coeff_out->values[0],  coeff_out->values[1],coeff_out->values[2],coeff_out->values[3],(int)pc_in->size());   
00153 
00154         if (ind_out->indices.size () < 10)
00155         {
00156                 ROS_WARN("Could not estimate a planar model for the given dataset.");
00157                 return 0;
00158         }
00159 
00160 
00161 
00162         return 1;}
00163 
00177 int c_polygon_primitive::indices_extraction(pcl::PointIndices::Ptr ind,
00178                 pcl::PointCloud<pcl::PointXYZ> *input_cloud,
00179                 pcl::PointCloud<pcl::PointXYZ> *remove_cloud,
00180                 pcl::PointCloud<pcl::PointXYZ>::Ptr copy_cloud,
00181                 pcl::PointCloud<pcl::Normal> *input_normals,
00182                 pcl::PointCloud<pcl::Normal> *remove_normals,
00183                 int compute_normals)
00184 {
00185 
00186         if (input_cloud!=NULL) //if an input cloud is given
00187         {
00188                 pcl::ExtractIndices<pcl::PointXYZ> extract; //Create the extraction object
00189                 extract.setInputCloud(input_cloud->makeShared());
00190                 extract.setIndices(ind);
00191 
00192                 //Copy to copy_cloud if not NULL 
00193                 if(copy_cloud!=NULL)
00194                 {
00195                         extract.setNegative(false);
00196                         extract.filter(*copy_cloud);
00197                 }
00198 
00199                 //Remove from remove if not NULL
00200                 if(remove_cloud!=NULL)
00201                 {
00202                         extract.setNegative(true);
00203                         extract.filter(*remove_cloud);
00204                 }
00205         }
00206 
00207         if (compute_normals==1)
00208                 if (input_normals!=NULL)
00209                 {
00210                         pcl::ExtractIndices<pcl::Normal> extract_normals; //Create the normal extraction object
00211                         extract_normals.setInputCloud(input_normals->makeShared());
00212                         extract_normals.setIndices(ind);
00213 
00214                         if (remove_normals!=NULL)
00215                         {
00216                                 extract_normals.setNegative(true);
00217                                 extract_normals.filter(*remove_normals);
00218                         }
00219                 } 
00220 
00221         return 1;}
00222 
00231         void c_polygon_primitive::create_reference_frame_from_plane_and_two_points(
00232                         pcl::ModelCoefficients::Ptr plane,
00233                         pcl::PointXYZ *pt1,
00234                         pcl::PointXYZ *pt2,
00235                         t_reference_frame *frame
00236                         )
00237 {
00238         //STEP1. Project the two points to the plane
00239         pcl::PointXYZ pt1_projected, pt2_projected;
00240         project_point_to_plane(pt1, plane, &pt1_projected);
00241         project_point_to_plane(pt2, plane, &pt2_projected);
00242 
00243         //STEP2. Check if the points are cohincident. If so cannot compute
00244         if (pt1_projected.x == pt2_projected.x && 
00245                         pt1_projected.y == pt2_projected.y && 
00246                         pt1_projected.z == pt2_projected.z)     
00247         {
00248                 ROS_ERROR("Cannot create reference frame. pt1 and pt2 after projection are cohincident.");
00249         }
00250 
00251         //STEP3. find the nsa vectors. a is given by the normal vector to the plane
00252         double a[3] = {plane->values[0],plane->values[1],plane->values[2]};
00253 
00254         //STEP4. Find the nsa vectors. the n vector will be the vector from pt1_projected to pt2_projected 
00255         double n[3]={pt2_projected.x - pt1_projected.x, pt2_projected.y - pt1_projected.y, pt2_projected.z - pt1_projected.z};
00256 
00257         //STEP5. Find the nsa vectors. The s vector is given by the external product a*n
00258         double s[3]={a[1]*n[2] - a[2]*n[1],     a[2]*n[0] - a[0]*n[2], a[0]*n[1] - a[1]*n[0]};
00259 
00260         //STEP6. Normalize all vectors
00261         normalize_vector(n); normalize_vector(s); normalize_vector(a);
00262 
00263         //STEP7. Compute the transform. The orientation given by nsa and the origin by pt1
00264         frame->transform = tf::Transform(tf::Matrix3x3(n[0], s[0] , a[0],
00265                                 n[1], s[1] , a[1],  
00266                                 n[2], s[2] , a[2]),  
00267                         tf::Vector3(pt1_projected.x,
00268                                 pt1_projected.y,
00269                                 pt1_projected.z));
00270 
00271         compute_arrow_points_from_transform(frame);
00272 }
00273 
00281 void c_polygon_primitive::project_point_to_plane(const pcl::PointXYZ *ptin, const pcl::ModelCoefficients::Ptr coeff, pcl::PointXYZ *ptout)
00282 {
00283         pcl::PointCloud<pcl::PointXYZ>::Ptr pcin = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
00284         pcin->points.push_back(*ptin);
00285 
00286         pcl::PointCloud<pcl::PointXYZ>::Ptr pcout = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
00287         //
00289         //float a=coeff->values[0];
00290         //float b=coeff->values[1];
00291         //float c=coeff->values[2];
00292         //float d=coeff->values[3];
00293 
00294         //float u=ptin->x;
00295         //float v=ptin->y;
00296         //float w=ptin->z;
00297 
00298         //float t0 = (a*u + b*v + c*w + d)/(a*a+b*b+c*c);
00299         //PFLN
00300 
00301         //ptout->x = u - a*t0;
00302         //ptout->y = v - b*t0;
00303         //ptout->z = w - c*t0;
00304 
00305 
00306         //float val = (u-a*t0)*a + (v-b*t0)*b + (w-c*t0)*c +d;
00307         //printf("new val=%f\n",val);
00308         //printf("coeff A=%f B=%f C=%f D=%f\n", coeff->values[0], coeff->values[1],coeff->values[2],coeff->values[3]);
00309 
00310         //printf("coeff sqrt(A2+B2+C2)=%f\n", sqrt(coeff->values[0]*coeff->values[0] + coeff->values[1]*coeff->values[1] +coeff->values[2]*coeff->values[2]));
00311         //PFLN
00312         //Create the projection object
00313         pcl::ProjectInliers<pcl::PointXYZ> projection;
00314         projection.setModelType(pcl::SACMODEL_PLANE); //set model type
00315         //projection.setModelType(pcl::SACMODEL_NORMAL_PARALLEL_PLANE); //set model type
00316 
00317         projection.setInputCloud(pcin);
00318         projection.setModelCoefficients(coeff);
00319         projection.filter(*pcout);
00320 
00321         *ptout = pcout->points[0];
00322 
00323         if(!check_if_point_lies_on_plane(coeff, ptout))
00324         {
00325                 //PFLN
00326                 ROS_ERROR("Error in projection of point. \nPoint x=%f y=%f z=%f does not lie on plane A=%f B=%f C=%f D=%f",ptout->x, ptout->y, ptout->z, coeff->values[0], coeff->values[1], coeff->values[2], coeff->values[3]);
00327                 exit(0);
00328         }
00329 
00330         pcin.reset();
00331         pcout.reset();
00332 }
00333 
00342 bool c_polygon_primitive::check_if_point_lies_on_plane(const pcl::ModelCoefficients::Ptr plane, const pcl::PointXYZ *p)
00343 {
00344         double val = plane->values[0]*p->x + plane->values[1]*p->y + plane->values[2]*p->z + plane->values[3];  
00345 
00346 
00347         if (val> -0.001 && val < 0.001) //Dont know why this happens. Floating point precision?
00348                 return true;
00349         else
00350         {
00351                 //ROS_INFO("val = %f",val);
00352                 return false;
00353         }
00354 }
00355 
00367 int c_polygon_primitive::check_plane_normal_orientation(pcl::ModelCoefficients::Ptr plane, pcl::PointXYZ *point, double vx, double vy, double vz)
00368 {       
00369         //Receive a plane equation in Hessian form Ax+By+Cz+D=0
00370         //And a point xyz
00371         //And an arbitrary viewpoint vx vy vz
00372         //Must check if the Normal is oriented towards the viewpoint. If not reorient it
00373 
00374         //STEP1. Backup the plane coefficients
00375         pcl::ModelCoefficients::Ptr plane_old; //create a backup struct
00376         plane_old = pcl::ModelCoefficients::Ptr(new pcl::ModelCoefficients);
00377         plane_old->values.resize(4); //allocate mem
00378         copy(plane, plane_old); //copy to the backup
00379 
00380         //STEP2. Obtain the projection of point onto the plane
00381 
00382         pcl::PointXYZ point_projected;
00383         project_point_to_plane(point, plane, &point_projected);
00384 
00385 
00386         //STEP3. Flip normal if necessary
00387         pcl::flipNormalTowardsViewpoint (point_projected, vx, vy, vz, plane->values[0], plane->values[1], plane->values[2]);
00388 
00389         //STEP4. If there was a flip must recompute parameter D
00390         if (plane_old->values[0]!=plane->values[0] ||
00391                         plane_old->values[1]!=plane->values[1] ||
00392                         plane_old->values[2]!=plane->values[2])
00393         {
00394                 //ROS_WARN("Normal was fliped!");
00395 
00396                 //Rcompute the new plane->values[3] using D = -Ax-By-Cz 
00397                 plane->values[3] = -plane->values[0]*point_projected.x 
00398                         -plane->values[1]*point_projected.y 
00399                         -plane->values[2]*point_projected.z; 
00400 
00401                 //ROS_INFO("Before flip A=%f B=%f C=%f D=%f Non-projected point x=%f y=%f z=%f lies on plane %d", plane->values[0], plane->values[1], plane->values[2], plane->values[3], point->x, point->y, point->z, check_if_point_lies_on_plane(plane, point));    
00402                 //ROS_INFO("Before flip A=%f B=%f C=%f D=%f Projected point x=%f y=%f z=%f lies on plane %d", plane->values[0], plane->values[1], plane->values[2], plane->values[3], point_projected.x, point_projected.y, point_projected.z, check_if_point_lies_on_plane(plane, &point_projected));  
00403         }
00404         else
00405         {
00406                 //ROS_INFO("Normal was NOT fliped!");
00407         }
00408 
00409         plane_old.reset();
00410         return 1;}
00411 
00412 
00418 void c_polygon_primitive::compute_arrow_points_from_transform(t_reference_frame *frame)
00419 {
00420         pcl::PointCloud<pcl::PointXYZ> pc_in;
00421         pcl::PointCloud<pcl::PointXYZ> pc_out;
00422 
00423         pcl::PointXYZ p;
00424         double arrow_size = 1.5;
00425 
00426         p.x = arrow_size ; p.y=0; p.z=0; 
00427         pc_in.points.push_back(p);
00428         p.x = 0; p.y=arrow_size; p.z=0; 
00429         pc_in.points.push_back(p);
00430         p.x = 0; p.y=0; p.z=arrow_size; 
00431         pc_in.points.push_back(p);
00432         p.x = 0; p.y=0; p.z=0; 
00433         pc_in.points.push_back(p);
00434 
00435         //transform_pc(pc_in, pc_out, &frame->transform);
00436         pcl_ros::transformPointCloud(pc_in, pc_out,  frame->transform);
00437 
00438         frame->arrow_x.x = pc_out.points[0].x;
00439         frame->arrow_x.y = pc_out.points[0].y;
00440         frame->arrow_x.z = pc_out.points[0].z;
00441 
00442         frame->arrow_y.x = pc_out.points[1].x;
00443         frame->arrow_y.y = pc_out.points[1].y;
00444         frame->arrow_y.z = pc_out.points[1].z;
00445 
00446         frame->arrow_z.x = pc_out.points[2].x;
00447         frame->arrow_z.y = pc_out.points[2].y;
00448         frame->arrow_z.z = pc_out.points[2].z;
00449 
00450         frame->origin.x = pc_out.points[3].x;
00451         frame->origin.y = pc_out.points[3].y;
00452         frame->origin.z = pc_out.points[3].z;
00453 
00454 }
00455 
00456 
00457 
00458 
00459 
00460 
00461 
00462 
00463 
00471 void c_polygon_primitive::refine_plane_coefficients(pcl::PointCloud<pcl::PointXYZ>::Ptr pcin, pcl::ModelCoefficients::Ptr coeff)
00472 {
00473         //Create the indices object
00474         pcl::PointIndices::Ptr indices = pcl::PointIndices::Ptr(new pcl::PointIndices); 
00475         // Create the segmentation object
00476         pcl::SACSegmentation<pcl::PointXYZ> seg;
00477 
00478         //Setup segmentation parameters
00479         seg.setOptimizeCoefficients(true);
00480         seg.setModelType(pcl::SACMODEL_PLANE);
00481         seg.setMethodType (pcl::SAC_RANSAC); 
00482         seg.setMaxIterations (50);
00483         seg.setDistanceThreshold(99999); //with this huge value all points are considered
00484         seg.setInputCloud(pcin->makeShared());
00485 
00486         //Compute  a plane candidate from the point cloud
00487         seg.segment(*indices, *coeff);
00488 
00489         indices.reset(); //free the indices object
00490 }
00491 
00499 void c_polygon_primitive::project_pc_to_plane(pcl::PointCloud<pcl::PointXYZ>::Ptr pcin, pcl::ModelCoefficients::Ptr coeff, pcl::PointCloud<pcl::PointXYZ>::Ptr pcout)
00500 {
00501         //Create the projection object
00502         pcl::ProjectInliers<pcl::PointXYZ> projection;
00503 
00504         projection.setModelType(pcl::SACMODEL_NORMAL_PLANE); //set model type
00505         projection.setInputCloud(pcin);
00506         projection.setModelCoefficients(coeff);
00507         projection.filter(*pcout);
00508 }
00509 
00515 void c_polygon_primitive::set_names(const char* s)
00516 { 
00517         sprintf(data.misc.name,"%s",s);
00518 }
00519 
00523 void c_polygon_primitive::set_reference_systems(void)
00524 {
00525         pointclouds.all->header.frame_id = data.frames.global_name;
00526         pointclouds.additional->header.frame_id = data.frames.global_name; 
00527         pointclouds.projected->header.frame_id = data.frames.global_name; 
00528         data.hulls.convex.polygon->header.frame_id = data.frames.global_name; 
00529         data.hulls.convex.extended_polygon->header.frame_id = data.frames.global_name; 
00530         data.hulls.concave.polygon->header.frame_id = data.frames.global_name; 
00531         data.hulls.concave.extended_polygon->header.frame_id = data.frames.global_name; 
00532         pointclouds.growed->header.frame_id = data.frames.global_name; 
00533         pointclouds.tmp->header.frame_id = data.frames.global_name; 
00534 }
00535 
00536 void c_polygon_primitive::allocate_space(void)
00537 {
00538         pointclouds.all= pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
00539         pointclouds.projected= pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
00540         pointclouds.additional = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
00541         data.hulls.convex.polygon = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
00542         data.hulls.convex.extended_polygon = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
00543         data.hulls.concave.polygon = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
00544         data.hulls.concave.extended_polygon = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
00545         pointclouds.growed = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
00546         pointclouds.tmp = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
00547 
00548         data.planes.current = pcl::ModelCoefficients::Ptr(new pcl::ModelCoefficients);
00549         data.planes.current->values.resize(4);
00550         data.planes.previous = pcl::ModelCoefficients::Ptr(new pcl::ModelCoefficients);
00551         data.planes.previous->values.resize(4);
00552 }
00553 
00559 void c_polygon_primitive::normalize_vector(double *v)
00560 {
00561 
00562         double n = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
00563 
00564         v[0] = v[0]/n;
00565         v[1] = v[1]/n;
00566         v[2] = v[2]/n;
00567 
00568 }
00569 
00570 int c_polygon_primitive::print_polygon_information(void)
00571 {
00572         ROS_INFO("%s INFORMATION\n \
00573                         Color rgb [%d,%d,%d]\n \
00574                         Support plane is A=%3.2f B=%3.2f C=%3.2f D=%3.2f \n \
00575                         Number of support points %d \n \
00576                         Convex Hull with %d points, Area of %3.2f and Solidity of %3.2f \n \
00577                         Concave Hull with %d points, Area of %3.2f and Solidity of %3.2f \n \
00578                         ", data.misc.name, data.misc.color.r, data.misc.color.g, data.misc.color.b,  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.);
00579 
00580         //ROS_INFO("Transform T-1 Origin = [%3.4f, %3.4f, %3.4f]",
00581                         //(data.frames.previous.transform.getOrigin()).x(),
00582                         //(data.frames.previous.transform.getOrigin()).y(),
00583                         //(data.frames.previous.transform.getOrigin()).z()
00584                         //);
00585 
00586         //ROS_INFO("Transform T-1 Quat = [%3.4f, %3.4f, %3.4f, %3.4f]",
00587                         //(data.frames.previous.transform.getRotation()).x(),
00588                         //(data.frames.previous.transform.getRotation()).y(),
00589                         //(data.frames.previous.transform.getRotation()).z(),
00590                         //(data.frames.previous.transform.getRotation()).w()
00591                         //);
00592 
00593         //ROS_INFO("Transform T Origin = [%3.4f, %3.4f, %3.4f]",
00594                         //(data.frames.current.transform.getOrigin()).x(),
00595                         //(data.frames.current.transform.getOrigin()).y(),
00596                         //(data.frames.current.transform.getOrigin()).z()
00597                         //);
00598 
00599         //ROS_INFO("Transform T Quat = [%3.4f, %3.4f, %3.4f, %3.4f]",
00600                         //(data.frames.current.transform.getRotation()).x(),
00601                         //(data.frames.current.transform.getRotation()).y(),
00602                         //(data.frames.current.transform.getRotation()).z(),
00603                         //(data.frames.current.transform.getRotation()).w()
00604                         //);
00605 
00606         return 1;
00607 }
00608 
00609 
00610 #endif
00611 


polygon_primitives_extraction
Author(s): Miguel Oliveira
autogenerated on Thu Nov 20 2014 11:35:55