00001 00060 #ifndef __FOV_SEGMENTATION_H__ 00061 #define __FOV_SEGMENTATION_H__ 00062 00063 //################## 00064 //#### includes #### 00065 00066 /*#include <Eigen/Core> 00067 #include <Eigen/Geometry> 00068 #include <iostream>*/ 00069 #include <cob_3d_mapping_common/polygon.h> 00070 #include <cob_3d_fov_segmentation/field_of_view.h> 00071 00072 namespace cob_3d_mapping 00073 { 00074 00078 class FOVSegmentation 00079 { 00080 public: 00081 typedef std::vector<Polygon::Ptr>::iterator polygon_iterator; 00083 FOVSegmentation(); 00084 00085 00087 ~FOVSegmentation() 00088 { 00090 } 00091 00097 void setFOV(FieldOfView& fov) { 00098 fov_ = fov; 00099 } 00100 00106 void setShapeArray(std::vector<Polygon::Ptr>& polygons) 00107 { 00108 polygons_in_ = polygons; 00109 } 00110 00111 00117 void compute(std::vector<Polygon::Ptr>& polygons); 00118 00119 00120 00121 protected: 00131 bool ccw(Eigen::Vector2f& a, Eigen::Vector2f& b, Eigen::Vector2f& c); 00132 00139 void clipFOVandPlane(Polygon::Ptr& poly, std::vector<Eigen::Vector3f>& intersections); 00140 00141 std::vector<Polygon::Ptr> polygons_in_; 00142 //std::vector<Polygon::Ptr> polygons_fov_; 00143 FieldOfView fov_; 00144 00145 }; 00146 00147 00148 } //namespace 00149 00150 #endif //__FOV_SEGMENTATION_H__ 00151