/home/joel/ros_workspace/lar3/utils/points_from_volume/include/points_from_volume/points_from_volume.h

Go to the documentation of this file.

#ifndef _points_from_volume_lib_H_
#define _points_from_volume_lib_H_


//INCLUDES
#include <ros/ros.h>
#include <stdio.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/transforms.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/project_inliers.h>
typedef enum {SUCCESS, FAILURE}t_func_output;

#define _CONVEX_LIB_DEBUG_ 0

using namespace std;

template<class T>
class points_from_volume
{

public:

        points_from_volume()
        {
                flag_convexhull_set=false;
        };

        ~points_from_volume()
        {
#if _CONVEX_LIB_DEBUG_ 
printf("Destructor...\n");
#endif
        };

        
        t_func_output set_convex_hull(pcl::PointCloud<T>& pc_in)
        {
                convex_hull=pc_in;
                convex_hull.header.frame_id = pc_in.header.frame_id;
                #if _CONVEX_LIB_DEBUG_
                        cout<<"pc_in frame id on set_convex_hul -> "<<pc_in.header.frame_id<<endl;
                #endif
                flag_convexhull_set=true;
                return SUCCESS;
        };
        
        t_func_output set_convex_hull(const sensor_msgs::PointCloud2ConstPtr& pcmsg_in)
        {
                pcl::PointCloud<T> pc_in;
                pcl::fromROSMsg(*pcmsg_in,pc_in);
                return set_convex_hull(pc_in);
        };
        
        pcl::PointCloud<T> get_pc_in_volume()
        {
                return pc_in_volume;
        };
        
        t_func_output convexhull_function(pcl::PointCloud<T>& pc_in, double positive_offset, double negative_offset, bool flag_in_out)
        {
                if(!flag_convexhull_set)
                {
                        ROS_ERROR("Flag not set to TRUE");
                        return FAILURE;
                }
                
                if(pc_in.header.frame_id!=convex_hull.header.frame_id)
                {
#if _CONVEX_LIB_DEBUG_
cout<<"convex_hull frame id "<<convex_hull.header.frame_id<<endl;
cout<<"pc_in frame id on convexhull_function"<<pc_in.header.frame_id<<endl;
#endif
                        ROS_ERROR("Non-matching frame id's");
                        return FAILURE;
                }
                
                pcl::ExtractPolygonalPrismData<T> epp;                                
                typename pcl::PointCloud<T>::ConstPtr input_cloud_constptr;             
                input_cloud_constptr.reset (new pcl::PointCloud<T> (pc_in));
                typename pcl::PointCloud<T>::ConstPtr convex_hull_constptr;
                convex_hull_constptr.reset (new pcl::PointCloud<T> (convex_hull));

                pcl::ExtractIndices<T> extract; //Create the extraction object
                pcl::PointIndices::Ptr indices;
                indices.reset();
                indices = pcl::PointIndices::Ptr(new pcl::PointIndices); 

                pc_in_volume.header.frame_id=pc_in.header.frame_id;
                
                //Set epp parameters
                epp.setInputCloud(input_cloud_constptr);
                epp.setInputPlanarHull(convex_hull_constptr);
                epp.setHeightLimits(negative_offset,positive_offset); 
                epp.setViewPoint(0,0,0); //i dont think this serves any purpose in the case of epp
                epp.segment(*indices);
                
                pc_in_volume.points.clear();
                pc_in_volume.height=1;
                pc_in_volume.width=pc_in_volume.points.size();
                if ((int)indices->indices.size()!=0)
                {
                        extract.setInputCloud(pc_in.makeShared());
                        extract.setIndices(indices);
                        extract.setNegative(flag_in_out);
                        extract.filter(pc_in_volume);
                }
//              else
//              {
//                      extract.setInputCloud(pc_in.makeShared());
//                      extract.setIndices(indices);
//                      extract.setNegative(flag_in_out);
//                      extract.filter(pc_in_volume);
//                      
//              cout<<"NUMBER OF POINTS: "<<pc_in_volume.points.size()<<endl;
//              }
// cout<<"NUMBER OF POINTS: "<<pc_in_volume.points.size()<<endl;
                        input_cloud_constptr.reset();
                        convex_hull_constptr.reset();
                        indices.reset();
                return SUCCESS;
        }
        

private:
        pcl::PointCloud<T> pc_in_volume;
        bool flag_convexhull_set;
        pcl::PointCloud<T> convex_hull;
};


#endif