$search
00001 00034 #ifndef ROI_PCL_FILTER_H_ 00035 #define ROI_PCL_FILTER_H_ 00036 00037 00038 #include <pcl/point_cloud.h> 00039 #include <pcl/point_types.h> 00040 #include <pcl/filters/passthrough.h> 00041 #include <pcl/filters/extract_indices.h> 00042 #include <pcl/common/common.h> 00043 00044 00045 template <typename PointT> 00046 class ROI_Filter{ 00047 private: 00048 typedef pcl::PassThrough<PointT> PassThrough; 00049 typedef pcl::PointCloud<PointT> PointCloud; 00050 // typedef typename PointCloud::Ptr PointCloudPtr; 00051 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00052 PassThrough pass_x; 00053 PassThrough pass_y; 00054 PassThrough pass_z; 00055 Eigen::Vector4f min; 00056 Eigen::Vector4f max; 00057 00058 public: 00059 ROI_Filter(){} 00060 ROI_Filter(double min_x, double max_x, double min_y, double max_y, double min_z, double max_z){ 00061 setRoi(min_x, max_x, min_y, max_y, min_z, max_z); 00062 } 00063 void setRoi(double min_x, double max_x, double min_y, double max_y, double min_z, double max_z){ 00064 pass_x.setFilterFieldName ("x"); 00065 pass_x.setFilterLimits (min_x,max_x); 00066 pass_y.setFilterFieldName ("y"); 00067 pass_y.setFilterLimits (min_y,max_y); 00068 pass_z.setFilterFieldName ("z"); 00069 pass_z.setFilterLimits (min_z,max_z); 00070 min.x() = min_x; 00071 min.y() = min_y; 00072 min.z() = min_z; 00073 max.x() = max_x; 00074 max.y() = max_y; 00075 max.z() = max_z; 00076 } 00077 00078 void setRoi(Eigen::Matrix<double,3,2>& box ){ 00079 00080 Eigen::Vector3d lower = box.col(0).cwiseMin(box.col(1)); 00081 Eigen::Vector3d upper = box.col(0).cwiseMax(box.col(1)); 00082 00083 // std::cout << "lower\n" << lower <<std::endl; 00084 // std::cout << "upper\n" << upper <<std::endl; 00085 00086 pass_x.setFilterFieldName ("x"); 00087 pass_x.setFilterLimits (lower.x(),upper.x()); 00088 pass_y.setFilterFieldName ("y"); 00089 pass_y.setFilterLimits (lower.y(), upper.y()); 00090 pass_z.setFilterFieldName ("z"); 00091 pass_z.setFilterLimits (lower.z(),upper.z()); 00092 00093 min.block<3,1>(0,0) = lower.cast<float>(); 00094 max.block<3,1>(0,0) = upper.cast<float>(); 00095 } 00096 00097 00098 00099 void apply (const PointCloud& cloud_in, PointCloud& cloud_out ){ 00100 pcl::PointIndices indices; 00101 pcl::getPointsInBox(cloud_in,min,max,indices.indices); 00102 pcl::ExtractIndices<PointT> extract; 00103 extract.setInputCloud(boost::make_shared<pcl::PointCloud<PointT> >(cloud_in) ); 00104 extract.setIndices(boost::make_shared<pcl::PointIndices>(indices)); 00105 extract.filter(cloud_out); 00106 } 00107 00108 unsigned int getNumInROI(const PointCloud& cloud_in){ 00109 pcl::PointIndices indices; 00110 pcl::getPointsInBox(cloud_in,min,max,indices.indices); 00111 return indices.indices.size(); 00112 } 00113 00114 }; 00115 00116 00117 #endif /* ROI_PCL_FILTER_H_ */