organized_segmentation_demo.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  * 
00004  * Point Cloud Library (PCL) - www.pointclouds.org
00005  * Copyright (c) 2012-, Open Perception, Inc.
00006  * 
00007  * All rights reserved.
00008  * 
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions
00011  * are met: 
00012  * 
00013  *  * Redistributions of source code must retain the above copyright
00014  *    notice, this list of conditions and the following disclaimer.
00015  *  * Redistributions in binary form must reproduce the above
00016  *    copyright notice, this list of conditions and the following
00017  *    disclaimer in the documentation and/or other materials provided
00018  *    with the distribution.
00019  *  * Neither the name of the copyright holder(s) nor the names of its
00020  *    contributors may be used to endorse or promote products derived
00021  *    from this software without specific prior written permission.
00022  * 
00023  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  * POSSIBILITY OF SUCH DAMAGE.
00035  */
00036 
00037 #ifndef PCL_ORGANIZED_SEGMENTATION_DEMO_H_
00038 #define PCL_ORGANIZED_SEGMENTATION_DEMO_H_
00039 
00040 #include <pcl/apps/organized_segmentation_demo_qt.h>
00041 
00042 // Boost
00043 #include <boost/thread/thread.hpp>
00044 // PCL
00045 #include <pcl/point_cloud.h>
00046 #include <pcl/point_types.h>
00047 #include <pcl/io/openni_grabber.h>
00048 #include <pcl/io/oni_grabber.h>
00049 #include <pcl/io/pcd_grabber.h>
00050 #include <pcl/io/pcd_io.h>
00051 #include <pcl/common/time.h>
00052 #include <pcl/visualization/pcl_visualizer.h>
00053 #include <pcl/filters/voxel_grid.h>
00054 #include <pcl/features/integral_image_normal.h>
00055 #include <pcl/segmentation/organized_multi_plane_segmentation.h>
00056 #include <pcl/segmentation/planar_polygon_fusion.h>
00057 #include <pcl/common/transforms.h>
00058 #include <pcl/segmentation/plane_coefficient_comparator.h>
00059 #include <pcl/segmentation/euclidean_plane_coefficient_comparator.h>
00060 #include <pcl/segmentation/rgb_plane_coefficient_comparator.h>
00061 #include <pcl/segmentation/edge_aware_plane_comparator.h>
00062 #include <pcl/segmentation/euclidean_cluster_comparator.h>
00063 #include <pcl/segmentation/organized_connected_component_segmentation.h>
00064 
00065 typedef pcl::PointXYZRGBA PointT;
00066 
00067 // Useful macros
00068 #define FPS_CALC(_WHAT_) \
00069 do \
00070 { \
00071     static unsigned count = 0;\
00072     static double last = pcl::getTime ();\
00073     double now = pcl::getTime (); \
00074     ++count; \
00075     if (now - last >= 1.0) \
00076     { \
00077       std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(now - last) << " Hz" <<  std::endl; \
00078       count = 0; \
00079       last = now; \
00080     } \
00081 }while(false)
00082 
00083 namespace Ui
00084 {
00085   class MainWindow;
00086 }
00087 
00088 class OrganizedSegmentationDemo : public QMainWindow
00089 {
00090   Q_OBJECT
00091   public:
00092     typedef pcl::PointCloud<PointT> Cloud;
00093     typedef Cloud::Ptr CloudPtr;
00094     typedef Cloud::ConstPtr CloudConstPtr;
00095   
00096 
00097     OrganizedSegmentationDemo(pcl::Grabber& grabber);
00098 
00099     ~OrganizedSegmentationDemo ()
00100     {
00101       if(grabber_.isRunning())
00102         grabber_.stop();
00103     }
00104   
00105     void cloud_cb (const CloudConstPtr& cloud);
00106   
00107   protected:
00108     boost::shared_ptr<pcl::visualization::PCLVisualizer> vis_;
00109     pcl::Grabber& grabber_;
00110 
00111     QMutex mtx_;
00112     QMutex vis_mtx_;
00113     Ui::MainWindow *ui_;
00114     QTimer *vis_timer_;
00115     pcl::PointCloud<PointT> prev_cloud_;
00116     pcl::PointCloud<pcl::Normal> prev_normals_;
00117     std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > prev_regions_;
00118     float* prev_distance_map_;
00119     
00120     pcl::PointCloud<PointT>::CloudVectorType prev_clusters_;
00121     
00122     pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
00123     pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
00124 
00125     bool capture_;
00126     bool data_modified_;
00127     size_t previous_data_size_;
00128     size_t previous_clusters_size_;
00129 
00130     bool display_normals_;
00131     bool display_curvature_;
00132     bool display_distance_map_;
00133 
00134     bool use_planar_refinement_;
00135     bool use_clustering_;
00136 
00137     pcl::PlaneCoefficientComparator<PointT, pcl::Normal>::Ptr plane_comparator_;
00138     pcl::EuclideanPlaneCoefficientComparator<PointT, pcl::Normal>::Ptr euclidean_comparator_;
00139     pcl::RGBPlaneCoefficientComparator<PointT, pcl::Normal>::Ptr rgb_comparator_;
00140     pcl::RGBPlaneCoefficientComparator<PointT, pcl::Normal> rgb_comp_;
00141     pcl::EdgeAwarePlaneComparator<PointT, pcl::Normal>::Ptr edge_aware_comparator_;
00142     pcl::EuclideanClusterComparator<PointT, pcl::Normal, pcl::Label>::Ptr euclidean_cluster_comparator_;
00143 
00144   public slots:
00145     void toggleCapturePressed()
00146     {
00147       capture_ = !capture_;
00148     }
00149 
00150     void usePlaneComparatorPressed ();
00151     void useEuclideanComparatorPressed ();
00152     void useRGBComparatorPressed ();
00153     void useEdgeAwareComparatorPressed ();
00154     
00155     void displayCurvaturePressed ();
00156     void displayDistanceMapPressed ();
00157     void displayNormalsPressed ();
00158                                  
00159     void disableRefinementPressed ()
00160     {
00161       use_planar_refinement_ = false;
00162     }
00163     
00164     void usePlanarRefinementPressed ()
00165     {
00166       use_planar_refinement_ = true;
00167     }
00168 
00169     void disableClusteringPressed ()
00170     {
00171       use_clustering_ = false;
00172     }
00173 
00174     void useEuclideanClusteringPressed ()
00175     {
00176       use_clustering_ = true;
00177     }
00178     
00179 
00180   private slots:
00181   void
00182     timeoutSlot();
00183 
00184 };
00185 
00186 #endif    // PCL_ORGANIZED_SEGMENTATION_DEMO_H_


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:27:30