euclidean_cluster_comparator.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) 2010-2012, Willow Garage, 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 Willow Garage, Inc. 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  *
00038  */
00039 
00040 #ifndef PCL_SEGMENTATION_EUCLIDEAN_CLUSTER_COMPARATOR_H_
00041 #define PCL_SEGMENTATION_EUCLIDEAN_CLUSTER_COMPARATOR_H_
00042 
00043 #include <pcl/segmentation/comparator.h>
00044 #include <boost/make_shared.hpp>
00045 
00046 namespace pcl
00047 {
00053   template<typename PointT, typename PointNT, typename PointLT>
00054   class EuclideanClusterComparator: public Comparator<PointT>
00055   {
00056     public:
00057       typedef typename Comparator<PointT>::PointCloud PointCloud;
00058       typedef typename Comparator<PointT>::PointCloudConstPtr PointCloudConstPtr;
00059       
00060       typedef typename pcl::PointCloud<PointNT> PointCloudN;
00061       typedef typename PointCloudN::Ptr PointCloudNPtr;
00062       typedef typename PointCloudN::ConstPtr PointCloudNConstPtr;
00063       
00064       typedef typename pcl::PointCloud<PointLT> PointCloudL;
00065       typedef typename PointCloudL::Ptr PointCloudLPtr;
00066       typedef typename PointCloudL::ConstPtr PointCloudLConstPtr;
00067 
00068       typedef boost::shared_ptr<EuclideanClusterComparator<PointT, PointNT, PointLT> > Ptr;
00069       typedef boost::shared_ptr<const EuclideanClusterComparator<PointT, PointNT, PointLT> > ConstPtr;
00070 
00071       using pcl::Comparator<PointT>::input_;
00072       
00074       EuclideanClusterComparator ()
00075         : normals_ ()
00076         , angular_threshold_ (0.0f)
00077         , distance_threshold_ (0.005f)
00078         , depth_dependent_ ()
00079         , z_axis_ ()
00080       {
00081       }
00082       
00084       virtual
00085       ~EuclideanClusterComparator ()
00086       {
00087       }
00088 
00089       virtual void 
00090       setInputCloud (const PointCloudConstPtr& cloud)
00091       {
00092         input_ = cloud;
00093         Eigen::Matrix3f rot = input_->sensor_orientation_.toRotationMatrix ();
00094         z_axis_ = rot.col (2);
00095       }
00096       
00100       inline void
00101       setInputNormals (const PointCloudNConstPtr &normals)
00102       {
00103         normals_ = normals;
00104       }
00105 
00107       inline PointCloudNConstPtr
00108       getInputNormals () const
00109       {
00110         return (normals_);
00111       }
00112 
00116       virtual inline void
00117       setAngularThreshold (float angular_threshold)
00118       {
00119         angular_threshold_ = cosf (angular_threshold);
00120       }
00121       
00123       inline float
00124       getAngularThreshold () const
00125       {
00126         return (acos (angular_threshold_) );
00127       }
00128 
00132       inline void
00133       setDistanceThreshold (float distance_threshold, bool depth_dependent)
00134       {
00135         distance_threshold_ = distance_threshold;
00136         depth_dependent_ = depth_dependent;
00137       }
00138 
00140       inline float
00141       getDistanceThreshold () const
00142       {
00143         return (distance_threshold_);
00144       }
00145 
00149       void
00150       setLabels (PointCloudLPtr& labels)
00151       {
00152         labels_ = labels;
00153       }
00154 
00158       void
00159       setExcludeLabels (std::vector<bool>& exclude_labels)
00160       {
00161         exclude_labels_ = boost::make_shared<std::vector<bool> >(exclude_labels);
00162       }
00163 
00169       virtual bool
00170       compare (int idx1, int idx2) const
00171       {
00172         int label1 = labels_->points[idx1].label;
00173         int label2 = labels_->points[idx2].label;
00174         
00175         if (label1 == -1 || label2 == -1)
00176           return false;
00177         
00178         if ( (*exclude_labels_)[label1] || (*exclude_labels_)[label2])
00179           return false;
00180         
00181         float dx = input_->points[idx1].x - input_->points[idx2].x;
00182         float dy = input_->points[idx1].y - input_->points[idx2].y;
00183         float dz = input_->points[idx1].z - input_->points[idx2].z;
00184         float dist = sqrt (dx*dx + dy*dy + dz*dz);
00185 
00186         return (dist < distance_threshold_);
00187       }
00188       
00189     protected:
00190       PointCloudNConstPtr normals_;
00191       PointCloudLPtr labels_;
00192 
00193       boost::shared_ptr<std::vector<bool> > exclude_labels_;
00194       float angular_threshold_;
00195       float distance_threshold_;
00196       bool depth_dependent_;
00197       Eigen::Vector3f z_axis_;
00198   };
00199 }
00200 
00201 #endif // PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:14:52