polygon_filter.cpp

Go to the documentation of this file.

#include <algorithm> //min, max
#include <cstdlib> //rand
#include "polygon_filter.h"
template <class point_type1>
bool point_inside_polygon(const pcl::PointCloud<point_type1>& cropping_polygon, const point_type1& query_point, int x, int y)
{
  //For the following commenst, x is assumed horizontally and y vertically
  int num_intersections = 0;
  //Polygon Line Segment Start and End
  point_type1 p1,p2;

  for(unsigned int i = 0; i< cropping_polygon.size(); i++)
  {
    p1 = cropping_polygon[i];
    int next_vertex_idx = (i+1) % cropping_polygon.size();//Wrap around
    p2 = cropping_polygon[next_vertex_idx];
    //std::cout << "P1: "<< p1.x << ", " <<p1.y << "\n";
    //std::cout << "P2: "<< p2.x << ", " <<p2.y << "\n";
    //if query point is between lower ...
    if (query_point.data[y] > std::min(p1.data[y],p2.data[y])) {
      //...and upper segment end... 
      if (query_point.data[y] <= std::max(p1.data[y],p2.data[y])) {
        //Check in which half plane it is using the dot product of the segment's normal
        point_type1 segment_direction, normal;
        segment_direction.data[x] = p2.data[x]-p1.data[x];
        segment_direction.data[y] = p2.data[y]-p1.data[y];
        normal.data[x] = segment_direction.data[y];
        normal.data[y] = -segment_direction.data[x];
        //if normal points to the right, flip it to the left
        if(normal.data[x] > 0){
          normal.data[x] *= -1.0;
          normal.data[y] *= -1.0;
        }
        //std::cout << "N:  "<< normal.x << ", " <<normal.y << "\n";
        //Dot Product
        point_type1 query_point_rel;
        query_point_rel.data[x] = query_point.data[x] - p1.data[x];
        query_point_rel.data[y] = query_point.data[y] - p1.data[y];
        //std::cout << "QR: "<< query_point_rel.x << ", " <<query_point_rel.y << "\n";
        double dot = normal.data[x] * query_point_rel.data[x] + normal.data[y] * query_point_rel.data[y];
        if(dot > 0) //it's to the left, i.e. the ray 
          num_intersections++;
      }      
    }
  }

  if (num_intersections% 2 == 0)
    return false;
  else
    return true;
}

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, int y, bool invert)
{
  pcl::PointCloud<point_type1> cloud_inside;
  cloud_inside.header =  cloud_to_crop.header;

  for(unsigned int i = 0; i < cloud_to_crop.size(); i++)
  {
    if(point_inside_polygon(cropping_polygon, cloud_to_crop[i], x, y) xor invert)
    {
      cloud_inside.push_back(cloud_to_crop[i]);
    }
  }
  return cloud_inside;
}

void cropping_test(){

      pcl::PointCloud<pcl::PointXYZ> test_cloud;
      pcl::PointXYZ vertex;
      srand(time(NULL));
      for(int i = 0; i < 200; i++){
        vertex.x = rand()/(RAND_MAX/2.0); 
        vertex.y = rand()/(RAND_MAX/2.0); 
        test_cloud.points.push_back(vertex);
      }

      pcl::PointCloud<pcl::PointXYZ> cropping_polygon;
      vertex.x =  0.0; vertex.y =  0.0; cropping_polygon.points.push_back(vertex);//  +------+
      vertex.x =  1.0; vertex.y =  0.0; cropping_polygon.points.push_back(vertex);//  |      |
      vertex.x =  1.0; vertex.y =  1.0; cropping_polygon.points.push_back(vertex);//  |      |
      vertex.x =  0.0; vertex.y =  1.0; cropping_polygon.points.push_back(vertex);//  +------+
      
      pcl::PointCloud<pcl::PointXYZ> inside_cloud = crop_by_polygon(cropping_polygon, test_cloud, 0, 1);
      pcl::PointCloud<pcl::PointXYZ> outside_cloud = crop_by_polygon(cropping_polygon, test_cloud, 0, 1, true);
      for(unsigned int i = 0; i < inside_cloud.size(); i++){
        std::cout << "Point Inside (" << i << "/" << inside_cloud.size() << "): "<< inside_cloud.points[i].x << ", " <<inside_cloud.points[i].y << ", " <<inside_cloud.points[i].z << "\n";
      }
      std::cout << std::endl;
      for(unsigned int i = 0; i < outside_cloud.size(); i++){
        std::cout << "Point Outside (" << i << "/" << outside_cloud.size() << "): "<< outside_cloud.points[i].x << ", " <<outside_cloud.points[i].y << ", " <<outside_cloud.points[i].z << "\n";
      }
}