polygon_filter.h

Go to the documentation of this file.

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

template <class point_type1>
pcl::PointCloud<point_type1> crop_by_polygon(const pcl::PointCloud<point_type1>& cropping_polygon, const pcl::PointCloud<point_type1>& cloud_to_crop, int x = 0, int y = 1, bool invert = false);

template <class point_type1>
bool point_inside_polygon(const pcl::PointCloud<point_type1>& cropping_polygon, const point_type1& query_point, int x, int y);

template <class point_type1>
pcl::PointCloud<point_type1> standardCroppingPolygon(float door_radius){
    pcl::PointCloud<point_type1>  polygon;
    point_type1 inside_vertex, outside_vertex;
    float inside_radius = 0.15; //inner radius as to not see the door frame
    float outside_radius = door_radius-0.05; //same, but closer, to not miss the handle
    float min_angle =  -5.0/180*3.142; 
    float max_angle = 100.0/180*3.142; 
    float offset = - 0.05; // set the "center" back as to get a little margin
    for(float angle = min_angle; angle < max_angle; angle += 10.0/180.0*3.142){
      inside_vertex.x  = inside_radius  * cos(angle) + offset;
      inside_vertex.y  = inside_radius  * sin(angle) + offset;
      polygon.points.insert(polygon.end(), inside_vertex);

      outside_vertex.x = outside_radius * cos(angle) + offset;
      outside_vertex.y = outside_radius * sin(angle) + offset;
      polygon.points.insert(polygon.begin(), outside_vertex); 
    }
    return polygon;
      
}
template <class point_type1>
pcl::PointCloud<point_type1> standardCroppingPolygon2(float door_radius){
    pcl::PointCloud<point_type1> polygon;
    point_type1 vertex;
    float dr = door_radius-0.05; //margin to the door frame
    vertex.x =   0.20; vertex.y =   0.15; polygon.points.push_back(vertex);//a  d----------------c
    vertex.x =  -0.05; vertex.y =   0.15; polygon.points.push_back(vertex);//b  |               /
    vertex.x =  -0.50; vertex.y = door_radius; polygon.points.push_back(vertex);//c  |              /
    vertex.x = door_radius; vertex.y = door_radius; polygon.points.push_back(vertex);//d  |             /
    vertex.x = door_radius; vertex.y =  -0.10; polygon.points.push_back(vertex);//e  |         a--b
    vertex.x =   0.20; vertex.y =  -0.10; polygon.points.push_back(vertex);//f  e---------f
    return polygon;
}