cob_3d_mapping_common: cob_3d_mapping_common: cylinder.cpp Source File

00001 
00059 #include <ros/console.h>
00060 #include "cob_3d_mapping_common/cylinder.h"
00061 namespace cob_3d_mapping {
00062 
00063 //##############Methods to initialize cylinder and its paramers#########
00064 
00065 
00066 void
00067 Cylinder::ContoursFromCloud(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
00068 {
00069   std::vector<Eigen::Vector3f> pts;
00070   pts.resize(cloud->points.size());
00071   for(unsigned int j=0; j<cloud->points.size(); j++)
00072   {
00073 
00074     pts[j](0) = cloud->points[j].x;
00075     pts[j](1) = cloud->points[j].y;
00076     pts[j](2) = cloud->points[j].z;
00077   }
00078     contours.push_back(pts);
00079 }
00080 
00081 
00082 void
00083 Cylinder::ContoursFromList( std::vector<std::vector<Eigen::Vector3f> >& in_list)
00084 {
00085 
00086   computeAttributes(sym_axis, normal, origin_);
00087   contours.resize(in_list.size());
00088 
00089   for (size_t j = 0; j < in_list.size(); j++)
00090   {
00091 
00092     contours[j].resize(in_list[j].size());
00093     holes.resize(in_list[j].size());
00094     for (size_t k = 0; k < in_list[j].size(); k++)
00095     {
00096       contours[j][k] =in_list[j][k];
00097     }
00098   }
00099 }
00100 
00101 
00102 
00103 void
00104 Cylinder::ParamsFromCloud(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr in_cloud , std::vector<int>& indices)
00105 {
00106 
00107   //  Establish coordinate system for projection to horizontal plane
00108   Eigen::Affine3f trafo_hor2w;
00109   Eigen::Vector3f centroid3f ;
00110   centroid3f <<centroid[0] , centroid[1] , centroid[2];
00112   this->getTransformationFromPlaneToWorld(sym_axis,centroid3f,trafo_hor2w);
00113   pcl::PointCloud<pcl::PointXYZRGB>::Ptr trans_cloud( new pcl::PointCloud<pcl::PointXYZRGB>() );
00114   pcl::transformPointCloud(*in_cloud,indices,*trans_cloud,trafo_hor2w.inverse());
00115 
00116   // Inliers of circle model
00117   pcl::PointIndices inliers;
00118   // Coefficients of circle model
00119   pcl::ModelCoefficients coeff;
00120   // Create the segmentation object
00121   pcl::SACSegmentation<pcl::PointXYZRGB> seg;
00122   // Optimize coefficients
00123   seg.setOptimizeCoefficients (true);
00124   // Set type of method
00125   seg.setMethodType (pcl::SAC_MLESAC);
00126   // Set number of maximum iterations
00127   seg.setMaxIterations (10);
00128   // Set type of model
00129   seg.setModelType (pcl::SACMODEL_CIRCLE2D);
00130   // Set threshold of model
00131   seg.setDistanceThreshold (0.010);
00132   // Give as input the filtered point cloud
00133   seg.setInputCloud (trans_cloud);
00134   // Call the segmenting method
00135   seg.segment(inliers,coeff);
00136 
00137 
00138   //  origin in lcs
00139   Eigen::Vector3f l_origin,l_centroid;
00140   l_centroid = trafo_hor2w.inverse() * centroid3f;
00141   l_origin << coeff.values[0],coeff.values[1],l_centroid[2];
00142 
00143   //Parameters  in wcs
00144 
00145   origin_ = trafo_hor2w * l_origin  ;
00146   normal= centroid3f - origin_;
00147   r_=coeff.values[2];
00148   sym_axis.normalize();
00149   normal.normalize();
00150   this->computeAttributes(sym_axis, normal, origin_);
00151 }
00152 
00153 void
00154 Cylinder::ParamsFromShapeMsg()
00155 {
00156 
00157   if (sym_axis[2] < 0 )
00158   {
00159     sym_axis= sym_axis *-1;
00160   }
00161 
00162   sym_axis.normalize();
00163   normal.normalize();
00164   this->computeAttributes(sym_axis, normal, origin_);
00165 }
00166 
00167 void
00168 Cylinder::computeAttributes(const Eigen::Vector3f& sym_axis, const Eigen::Vector3f &new_normal, const Eigen::Vector3f& new_origin)
00169 {
00170   origin_=new_origin;
00171   this ->sym_axis = sym_axis;
00172   d=fabs(this->centroid.head(3).dot(normal));
00173   normal=new_normal;
00174 
00175   Eigen::Affine3f transform_from_plane_to_world;
00176   getTransformationFromPlaneToWorld(sym_axis,normal,origin_,transform_from_plane_to_world);
00177   transform_from_world_to_plane=transform_from_plane_to_world.inverse();
00178 
00179   //  calculation of h_min and h_max
00180   double min,max;
00181   min = 1000;
00182   max = -1000;
00183   std::vector<std::vector<Eigen::Vector3f> > trans_contours;
00184   this->getTransformedContours(this->transform_from_world_to_plane, trans_contours);
00185   for (size_t i = 0; i < trans_contours.size(); ++i) {
00186     for (size_t j = 0; j < trans_contours[i].size(); ++j) {
00187       if (trans_contours[i][j][1]< min) min = trans_contours[i][j][1];
00188       if (trans_contours[i][j][1]> max) max = trans_contours[i][j][1];
00189     }
00190   }
00191 
00192   h_max_ = max;
00193   h_min_ = min;
00194 }
00195 
00196 
00197 
00198 void
00199 Cylinder::transform2tf(Eigen::Affine3f & trafo)
00200 {
00201   //transform contours
00202   this->TransformContours(trafo);
00203 
00204   //  transform parameters
00205   Eigen::Vector3f tf_axes_1 = trafo.rotation() * this->sym_axis;
00206   this->sym_axis =  tf_axes_1;
00207 
00208   Eigen::Vector3f tf_axes_2 = trafo.rotation() * this->normal;
00209   this->normal = tf_axes_2;
00210 
00211   Eigen::Vector3f tf_origin = trafo * this->origin_;
00212   this->origin_ =  tf_origin;
00213 
00214   Eigen::Vector3f centroid3f;
00215   centroid3f<<  this->centroid[0], this->centroid[1], this->centroid[2];
00216   centroid3f = trafo * centroid3f;
00217   this->centroid << centroid3f[0], centroid3f[1], centroid3f[2], 0;
00218   this->computeAttributes(sym_axis,normal,origin_);
00219 
00220 }
00221 
00222 void
00223 Cylinder::GrabParams(Cylinder& c_src)
00224 {
00225   this->centroid = c_src.centroid;
00226   this->origin_ = c_src.origin_;
00227   this->d     = c_src.d;
00228   this->r_ = c_src.r_;
00229   this->transform_from_world_to_plane = c_src.transform_from_world_to_plane;
00230   this->normal = c_src.normal;
00231   this->sym_axis = c_src.sym_axis;
00232   this->h_max_ =c_src.h_max_;
00233   this->h_min_ =c_src.h_min_;
00234   this->merge_weight_ = c_src.merge_weight_;
00235   this->merged = c_src.merged;
00236   this->frame_stamp = c_src.frame_stamp;
00237   this->holes = c_src.holes;
00238   this->color=c_src.color;
00239 }
00240 
00241 
00242 //################## methods to roll and unroll cylinder###############
00243 
00244 void Cylinder::getCyl3D(Cylinder& c3d)
00245 {
00246 
00247   c3d= *this;
00248   c3d.makeCyl3D();
00249 
00250 }
00251 
00252 
00253 
00254 void Cylinder::makeCyl3D()
00255 {
00256   //Transform to local coordinate system
00257   Polygon poly_plane;
00258 
00259   for (size_t j = 0; j < contours.size(); j++) {
00260 
00261     poly_plane.holes.resize(contours.size());
00262     poly_plane.contours.resize(contours.size());
00263 
00264     for (size_t k = 0; k < contours[j].size(); k++) {
00265       poly_plane.contours[j].resize(contours[j].size());
00266 
00267       Eigen::Vector3f point_trans =
00268           transform_from_world_to_plane
00269           * contours[j][k];
00270 
00271       poly_plane.contours[j][k] = point_trans;
00272 
00273     }
00274   }
00275 
00276   // transform into cylinder shape via polar coordinates
00277   for (size_t j = 0; j < poly_plane.contours.size(); j++) {
00278 
00279     holes.resize(poly_plane.contours.size());
00280 
00281     for (size_t k = 0; k < poly_plane.contours[j].size(); k++) {
00282 
00283       Eigen::Vector3f point_temp;
00284       getPt3D(poly_plane.contours[j][k],point_temp);
00285       point_temp = transform_from_world_to_plane.inverse() * point_temp;
00286       contours[j][k] = point_temp;
00287     }
00288   }
00289 }
00290 
00291 void Cylinder::getPt3D(Eigen::Vector3f& pt2d,Eigen::Vector3f& pt3d){
00292 
00293       double alpha = pt2d[0]/ r_ ;
00294       //         use polar coordinates to create cylinder points
00295        pt3d<<  r_ * sin(-alpha), pt2d[1],  r_*  cos(-alpha);
00296 
00297 }
00298 
00299 void Cylinder::makeCyl2D()
00300 {
00301   bool start; // bool to indicate first point of contour
00302   float Tx_1,Tx_0;
00303   Eigen::Vector3f z_axis,p_0;
00304 
00305 
00306   for (size_t j = 0; j < contours.size(); j++)
00307   {
00308 
00309     Tx_0 = 0;
00310 
00311     for (size_t k = 0; k < contours[j].size(); k++)
00312     {
00313 
00314       Tx_1=0;
00315 
00316       //      Transform  Points in Cylinder Coordinate System
00317       Eigen::Vector3f point_trans =
00318           transform_from_world_to_plane * contours[j][k];
00319 
00320       if (k == 0) {
00321         z_axis << 0,0,1;
00322         start=true;
00323       }
00324 
00325       else {
00326         start = false;
00327         z_axis = transform_from_world_to_plane *z_axis;
00328       }
00329       getArc(point_trans,z_axis,Tx_1,start);
00330 
00331       // New coordinates( p_y = 0 because now on a plane)
00332       point_trans[0] =  Tx_0+ Tx_1;
00333       point_trans[2] = 0;
00334       Eigen::Vector3f point_global =
00335           transform_from_world_to_plane.inverse()
00336           * point_trans;
00337 
00338       Tx_0 = point_trans[0];
00339       z_axis =contours[j][k];
00340 
00341       contours[j][k] = point_global;
00342 
00343     }
00344   }
00345 }
00346 
00347 void
00348 Cylinder::getCyl2D(Cylinder& c2d)
00349 {
00350   c2d = *this;
00351   c2d.makeCyl2D();
00352 }
00353 
00354 
00355 //################## methods for merging############################
00356 
00357 void
00358 Cylinder::isMergeCandidate(const std::vector<Cylinder::Ptr>& cylinder_array,
00359     const merge_config& limits, std::vector<int>& intersections)
00360 {
00361 
00362   for (size_t i = 0; i < cylinder_array.size(); i++)
00363   {
00364     Cylinder& c_map = *(cylinder_array[i]);
00365     Eigen::Vector3f connection=c_map.origin_-origin_;
00366     connection.normalize();
00367     Eigen::Vector3f d= c_map.origin_  - this->origin_   ;
00368 
00369     // Test for geometrical attributes of cylinders
00370     if(fabs(c_map.sym_axis.dot(this->sym_axis)) > limits.angle_thresh && (fabs(c_map.r_ - r_) < (0.1 )))
00371     {
00372       // Test for spatial attributes of cylinders
00373       if( d.norm() < (c_map.r_+0.1) || fabs(c_map.sym_axis .dot(connection)) > limits.angle_thresh )
00374       {
00375         Cylinder::Ptr c1(new Cylinder);
00376         Cylinder::Ptr c2(new Cylinder);
00377         *c1 = *this;
00378 
00379 
00380         *c2= c_map;
00381         c2->transform_from_world_to_plane = c1->transform_from_world_to_plane;
00382         c1->makeCyl2D();
00383 
00384         c2->makeCyl2D();
00385 
00386         if (c1->isIntersectedWith(c2))
00387         {
00388           intersections.push_back(i);
00389         }
00390       }
00391     }
00392   }
00393 }
00394 
00395 
00396 void
00397 Cylinder::merge(std::vector<Cylinder::Ptr>& c_array)
00398 {
00399   ROS_DEBUG_STREAM("START MERGING");
00400   std::vector<Cylinder::Ptr> merge_cylinders;
00401 
00402   //create average cylinder for  averaging
00403   Cylinder::Ptr average_cyl =Cylinder::Ptr(new Cylinder());
00404   *average_cyl = *this;
00405   average_cyl->GrabParams(*this);
00406   average_cyl->applyWeighting(c_array);
00407 
00408   this->compensate_offset(average_cyl);
00409   this->makeCyl2D();
00410 
00411   for (int i = 0; i < (int) c_array.size(); i++)
00412   {
00413     c_array[i]->compensate_offset(average_cyl);
00414     c_array[i]->makeCyl2D();
00415     merge_cylinders.push_back(c_array[i]);
00416   }
00417 
00418   std::vector<Polygon::Ptr> merge_polygons;
00419   for (size_t i = 0; i < merge_cylinders.size(); ++i)
00420   {
00421     Polygon::Ptr tmp_ptr= merge_cylinders[i];
00422     merge_polygons.push_back(tmp_ptr);
00423   }
00424   Polygon::Ptr average_polygon= average_cyl;
00425 
00426   this->merge_union(merge_polygons,average_polygon);
00427   this->assignID(merge_polygons);
00428   this->GrabParams(*average_cyl);
00429   this->assignWeight();
00430   this->makeCyl3D();
00431 }
00432 
00433 
00434 
00435 
00436 void
00437 Cylinder::applyWeighting(std::vector<Cylinder::Ptr>& merge_candidates)
00438 {
00439 
00440   Eigen::Vector3f temp_sym_axis,temp_origin,temp_normal;
00441   temp_sym_axis = this->merge_weight_ * this->sym_axis;
00442   temp_origin = this->merge_weight_ * this->origin_;
00443   temp_normal = this->merge_weight_ * this->normal;
00444 
00445   double   merge_weight_sum = this ->merge_weight_;
00446   double temp_r = merge_weight_sum * this->r_;
00447   int merged_sum = this->merged;
00448 
00449 
00450   for (int i = 0; i < (int)merge_candidates.size(); ++i) {
00451 
00452 
00453     temp_sym_axis += merge_candidates[i]->merge_weight_ * merge_candidates[i]->sym_axis ;
00454     temp_normal += merge_candidates[i]->merge_weight_ * merge_candidates[i]->normal ;
00455     temp_origin += merge_candidates[i]->merge_weight_ * merge_candidates[i]->origin_;
00456     temp_r += merge_candidates[i]->merge_weight_ * merge_candidates[i]->r_;
00457     merge_weight_sum += merge_candidates[i]->merge_weight_;
00458     merged_sum  += merge_candidates[i]->merged;
00459 
00460     if (merge_candidates[i]->h_max_ > this-> h_max_)this->h_max_ = merge_candidates[i]->h_max_;
00461     if (merge_candidates[i]->h_min_  < this-> h_min_)this->h_min_ = merge_candidates[i]->h_min_;
00462 
00463   }
00464 
00465   this->sym_axis = temp_sym_axis / merge_weight_sum;
00466   this->normal = temp_normal / merge_weight_sum;
00467   this->origin_ = temp_origin / merge_weight_sum;
00468   this->sym_axis.normalize();
00469   this->r_ = temp_r / merge_weight_sum;
00470 
00471   if (merged_sum < merged_limit)
00472   {
00473     this->merged=merged_sum;
00474 
00475   }
00476   else
00477   {
00478     this->merged=merged_limit;
00479   }
00480 
00481   Eigen::Affine3f transform_from_plane_to_world;
00482   getTransformationFromPlaneToWorld(this->sym_axis,this->normal,this->origin_,transform_from_plane_to_world);
00483   this->transform_from_world_to_plane=transform_from_plane_to_world.inverse();
00484 }
00485 
00486 //##############Methods for Debug #################################################
00487 
00488 void
00489 Cylinder::dbg_out(pcl::PointCloud<pcl::PointXYZRGB>::Ptr points,std::string & name){
00490 
00491   std::ofstream os;
00492   std::string path = "/home/goa-tz/debug/";
00493   path.append(name.c_str());
00494   os.open(path.c_str());
00495 
00496   for (int i = 0; i < (int)points->width; ++i) {
00497     os << points->points[i].x;
00498     os <<" ";
00499     os << points->points[i].y;
00500     os <<" ";
00501     os << points->points[i].z;
00502     os <<"\n";
00503   }
00504 
00505   os.close();
00506 }
00507 
00508 
00509 void
00510 Cylinder::dump_params(std::string  name)
00511 {
00512 
00513   std::string path = "/home/goa-tz/eval/params/";
00514   path.append(name.c_str());
00515   std::ofstream os(path.c_str(),std::ofstream::app );
00516 
00517   os<<frame_stamp<<" "<<r_<<" "<<
00518   this->origin_[0]<<" "<<this->origin_[1]<<" "<<this->origin_[2]<<" "<<
00519   this->centroid[0]<<" "<<this->centroid[1]<<" "<<this->centroid[2]<<" "<<
00520   this->normal[0]<<" "<<this->normal[1]<<" "<<this->normal[2]<<" "<<
00521   this->sym_axis[0]<<" "<<this->sym_axis[1]<<" "<<this->sym_axis[2]<<"\n";
00522   os.close();
00523 }
00524 
00525 
00526 void
00527 Cylinder::printAttributes(std::string & name)
00528 {
00529   ROS_DEBUG_STREAM( "_______"<<name.c_str() <<"______\n");
00530   ROS_DEBUG_STREAM("origin = \n"<< this->origin_<<"\n");
00531   ROS_DEBUG_STREAM( "centroid = \n"<<this->centroid<<"\n");
00532   ROS_DEBUG_STREAM("radius = "<< this->r_<<"\n");
00533   ROS_DEBUG_STREAM("normal = "<< this->normal<<"\n");
00534   ROS_DEBUG_STREAM("sym_axis = "<< this->sym_axis<<"\n");
00535   ROS_DEBUG_STREAM("merged = " << this->merged<<"\n");
00536   ROS_DEBUG_STREAM("merge_weight = "<<this->merge_weight_<<"\n");
00537   ROS_DEBUG_STREAM( "_________________\n");
00538 }
00539 
00540 
00541 void Cylinder::getArc(const Eigen::Vector3f& goal,const Eigen::Vector3f& start, float& Tx,bool first)
00542 {
00543   Eigen::Vector2f a, b;
00544   a << start[0], start[2];
00545   b << goal[0],goal[2];
00546   a.normalize();
00547   b.normalize();
00548 
00549 
00550   double alpha = 0;
00551   double cos_alpha = a.dot(b) / (b.norm() * a.norm());
00552   if (cos_alpha > 0.99995)
00553   {
00554     Tx = 0;
00555   }
00556   else
00557   {
00558     if (first==true) {
00559         alpha = acos((fabs(cos_alpha)));
00560     }
00561     else
00562     {
00563       alpha = fabs(acos((cos_alpha)));
00564     }
00565 
00566     //        make sure alpha < M_PI
00567     if (alpha >= M_PI) {
00568       alpha -=M_PI;
00569     }
00570     Tx =  r_ *  alpha;
00571 
00572     Eigen::Vector2f  d_ba=b-a;
00573 
00574     //    1st section ---> above x axis
00575 
00576     if ((a[1]>=0 && b[1]>=0) && (a[0]<b[0]) ) {
00577       Tx*=-1;
00578     }
00579 
00580     //   2nd section --> positive x values
00581     else if ((a[0]>=0 && b[0]>=0) && (a[1]> b[1])) {
00582       Tx*=-1;
00583 
00584     }
00585 
00586     //    3rd section --> negative y values
00587     else if ((a[1]<=0 && b[1]<=0) && (a[0]>b[0])) {
00588       Tx*=-1;
00589 
00590     }
00591     //   4th section --> negative x values
00592     else if ((a[0]<=0 && b[0]<=0) && (a[1]< b[1])) {
00593       Tx*=-1;
00594     }
00595   }
00596 }
00597 
00598 void
00599 Cylinder::compensate_offset(Cylinder::Ptr& c_ref)
00600 {
00601     Eigen::Vector3f n12 = c_ref->transform_from_world_to_plane.rotation()* c_ref->normal;
00602     Eigen::Vector3f s12 = c_ref->transform_from_world_to_plane.rotation()* this->sym_axis;
00603     Eigen::Vector3f o12 = c_ref->transform_from_world_to_plane* this->origin_;
00604     o12[1]=0;
00605     Eigen::Affine3f T12;
00606     n12.normalize();
00607     s12.normalize();
00608     pcl::getTransformationFromTwoUnitVectorsAndOrigin(s12,n12,o12,T12);
00609     for (size_t i = 0; i < this->contours.size(); ++i)
00610     {
00611         for (size_t j = 0; j < this->contours[i].size(); ++j)
00612         {
00613             this->contours[i][j] = this-> transform_from_world_to_plane.inverse() * T12.inverse() * transform_from_world_to_plane * this->contours[i][j];
00614         }
00615     }
00616    this->transform_from_world_to_plane=c_ref->transform_from_world_to_plane;
00617 }
00618 
00619 
00620 }//namespace