$search
00001 00060 #ifndef CYLINDER_H_ 00061 #define CYLINDER_H_ 00062 00063 //general includes 00064 #include <math.h> 00065 #include <sstream> 00066 00067 //cob includes 00068 #include "cob_3d_mapping_common/shape.h" 00069 #include "cob_3d_mapping_common/polygon.h" 00070 extern "C" { 00071 #include "cob_3d_mapping_common/gpc.h" 00072 } 00073 //boost includes 00074 #include <boost/shared_ptr.hpp> 00075 #include <boost/enable_shared_from_this.hpp> 00076 #include <boost/lexical_cast.hpp> 00077 //pcl includes 00078 #include <pcl/point_cloud.h> 00079 #include <pcl/point_types.h> 00080 #include <pcl/common/centroid.h> 00081 #include <pcl/common/eigen.h> 00082 #ifdef PCL_VERSION_COMPARE 00083 #include <pcl/common/transforms.h> 00084 #else 00085 #include <pcl/common/transform.h> 00086 #endif 00087 #include <pcl/segmentation/sac_segmentation.h> 00088 #include <pcl/registration/transforms.h> 00089 #include <pcl/sample_consensus/method_types.h> 00090 #include <pcl/ModelCoefficients.h> 00091 #include <pcl/exceptions.h> 00092 #include <pcl/common/common.h> 00093 00094 00095 00096 00097 00098 00099 namespace cob_3d_mapping{ 00105 class Cylinder: public Polygon 00106 00107 { 00108 00109 public: 00114 typedef boost::shared_ptr<Cylinder> Ptr; 00115 00116 00117 00121 Cylinder():merged_limit(50) 00122 { 00123 } 00124 00125 //##############Methods to initialize cylinder and its paramers######### 00126 00131 void ContoursFromCloud(pcl::PointCloud<pcl::PointXYZ>::ConstPtr in_cloud); 00132 00133 00138 void ContoursFromList( std::vector<std::vector<Eigen::Vector3f> >& in_list); 00139 00140 00149 void ParamsFromCloud(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr in_cloud, std::vector<int>& indices); 00150 00151 00158 void ParamsFromShapeMsg(); 00159 00160 00169 virtual void computeAttributes(const Eigen::Vector3f & sym_axis,const Eigen::Vector3f &new_normal, const Eigen::Vector3f & new_origin); 00170 00171 00177 virtual void transform2tf(Eigen::Affine3f & tf); 00178 00179 00184 void GrabParams(Cylinder& c_src); 00185 00186 00187 //################## methods to roll and unroll cylinder############### 00195 void getCyl3D(Cylinder& c3d); 00196 00197 00203 void makeCyl3D(); 00204 00205 00212 void makeCyl2D(); 00213 00214 00222 void getCyl2D(Cylinder& c2d); 00223 00224 00236 void getPt3D(Eigen::Vector3f& pt2d,Eigen::Vector3f& pt3d); 00237 00238 00239 00240 //################## methods for merging############################ 00252 virtual void isMergeCandidate(const std::vector<Cylinder::Ptr >& cylinder_array,const merge_config& limits,std::vector<int>& intersections); 00253 00254 00265 virtual void merge(std::vector<Cylinder::Ptr >& c_array); 00266 00267 00275 virtual void applyWeighting(std::vector<Cylinder::Ptr >& merge_candidates); 00276 00277 00278 //############## debugging methods #################### 00284 void dbg_out(pcl::PointCloud<pcl::PointXYZRGB>::Ptr points,std::string& name); 00285 00286 00291 void printAttributes(std::string & name); 00292 00293 00298 void dump_params(std::string name); 00299 00300 00301 //################# member variables######################## 00302 double r_; 00303 double h_min_; 00304 double h_max_; 00305 Eigen::Vector3f sym_axis; 00306 Eigen::Vector3f origin_; 00307 int merged_limit; 00309 protected: 00310 //################ private methods for merging to avoid confusion by user################ 00321 void getArc(const Eigen::Vector3f& goal,const Eigen::Vector3f& start, float& Tx,bool first); 00322 00323 00333 void compensate_offset(Cylinder::Ptr& c_ref); 00334 00335 }; 00336 } 00337 00338 #endif