00001 00040 #ifndef VORONOI_APPROXIMATOR_IVSRUILH 00041 #define VORONOI_APPROXIMATOR_IVSRUILH 00042 00043 #include <bwi_mapper/map_loader.h> 00044 #include <bwi_mapper/map_inflator.h> 00045 #include <bwi_mapper/directed_dfs.h> 00046 #include <bwi_mapper/structures/point.h> 00047 #include <bwi_mapper/structures/voronoi_point.h> 00048 00049 namespace bwi_mapper { 00050 00051 00057 class VoronoiApproximator : public MapLoader { 00058 00059 public: 00060 00067 VoronoiApproximator(const std::string& fname) : 00068 MapLoader(fname), initialized_(false) {} 00069 00079 void findVoronoiPoints(double threshold, bool use_naive = false, 00080 int sub_pixel_sampling = 1); 00081 00087 void drawOutput(cv::Mat &image); 00088 00093 void printVoronoiPoints(); 00094 00099 void drawVoronoiPoints(cv::Mat &image, 00100 uint32_t orig_x = 0, uint32_t orig_y = 0); 00101 00102 protected: 00103 00105 std::vector<VoronoiPoint> voronoi_points_; 00106 00108 nav_msgs::OccupancyGrid inflated_map_; 00109 00111 bool initialized_; 00112 00113 }; /* VoronoiApproximator */ 00114 00115 } /* bwi_mapper */ 00116 00117 #endif /* end of include guard: VORONOI_APPROXIMATOR_IVSRUILH */