00001 #ifndef _SURFACE_PERCEPTION_SURFACE_FINDER_H_ 00002 #define _SURFACE_PERCEPTION_SURFACE_FINDER_H_ 00003 00004 #include <map> 00005 #include <vector> 00006 00007 #include "pcl/ModelCoefficients.h" 00008 #include "pcl/PointIndices.h" 00009 #include "pcl/point_cloud.h" 00010 #include "pcl/point_types.h" 00011 00012 namespace surface_perception { 00040 class SurfaceFinder { 00041 public: 00043 SurfaceFinder(); 00044 00049 void set_cloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud); 00050 00054 void set_cloud_indices(const pcl::PointIndices::Ptr cloud_indices); 00055 00063 void set_angle_tolerance_degree(double angle_tolerance_degree); 00064 00070 void set_max_point_distance(double max_point_distance); 00071 00080 void set_min_iteration(int min_iteration); 00081 00091 void set_surface_point_threshold(int surface_point_threshold); 00092 00098 void set_min_surface_amount(int min_surface_amount); 00099 00105 void set_max_surface_amount(int max_surface_amount); 00106 00121 void ExploreSurfaces(std::vector<pcl::PointIndices::Ptr>* indices_vec, 00122 std::vector<pcl::ModelCoefficients>* coeffs_vec); 00123 00124 private: 00125 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_; 00126 pcl::PointIndices::Ptr cloud_indices_; 00127 double angle_tolerance_degree_; 00128 double max_point_distance_; 00129 size_t min_iteration_; 00130 size_t surface_point_threshold_; 00131 size_t min_surface_amount_; 00132 size_t max_surface_amount_; 00133 std::map<double, std::vector<int> > sorted_indices_; 00134 void SortIndices(); 00135 void FitSurface(const pcl::PointIndices::Ptr old_indices_ptr, 00136 const pcl::ModelCoefficients::Ptr old_coeff_ptr, 00137 pcl::PointIndices::Ptr new_indices_ptr, 00138 pcl::ModelCoefficients::Ptr new_coeff_ptr); 00139 }; 00140 } // namespace surface_perception 00141 00142 #endif // _SURFACE_PERCEPTION_SURFACE_FINDER_H_