Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/segmentation/include/pcl/segmentation/euclidean_cluster_comparator.h
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 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  *
00038  */
00039 
00040 #ifndef PCL_SEGMENTATION_EUCLIDEAN_CLUSTER_COMPARATOR_H_
00041 #define PCL_SEGMENTATION_EUCLIDEAN_CLUSTER_COMPARATOR_H_
00042 
00043 #include <pcl/segmentation/boost.h>
00044 #include <pcl/segmentation/comparator.h>
00045 
00046 namespace pcl
00047 {
00048   /** \brief EuclideanClusterComparator is a comparator used for finding clusters supported by planar surfaces.
00049     * This needs to be run as a second pass after extracting planar surfaces, using MultiPlaneSegmentation for example.
00050     *
00051     * \author Alex Trevor
00052     */
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       
00073       /** \brief Empty constructor for EuclideanClusterComparator. */
00074       EuclideanClusterComparator ()
00075         : normals_ ()
00076         , angular_threshold_ (0.0f)
00077         , distance_threshold_ (0.005f)
00078         , depth_dependent_ ()
00079         , z_axis_ ()
00080       {
00081       }
00082       
00083       /** \brief Destructor for EuclideanClusterComparator. */
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       
00097       /** \brief Provide a pointer to the input normals.
00098         * \param[in] normals the input normal cloud
00099         */
00100       inline void
00101       setInputNormals (const PointCloudNConstPtr &normals)
00102       {
00103         normals_ = normals;
00104       }
00105 
00106       /** \brief Get the input normals. */
00107       inline PointCloudNConstPtr
00108       getInputNormals () const
00109       {
00110         return (normals_);
00111       }
00112 
00113       /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
00114         * \param[in] angular_threshold the tolerance in radians
00115         */
00116       virtual inline void
00117       setAngularThreshold (float angular_threshold)
00118       {
00119         angular_threshold_ = cosf (angular_threshold);
00120       }
00121       
00122       /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
00123       inline float
00124       getAngularThreshold () const
00125       {
00126         return (acos (angular_threshold_) );
00127       }
00128 
00129       /** \brief Set the tolerance in meters for difference in perpendicular distance (d component of plane equation) to the plane between neighboring points, to be considered part of the same plane.
00130         * \param[in] distance_threshold the tolerance in meters
00131         */
00132       inline void
00133       setDistanceThreshold (float distance_threshold, bool depth_dependent)
00134       {
00135         distance_threshold_ = distance_threshold;
00136         depth_dependent_ = depth_dependent;
00137       }
00138 
00139       /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
00140       inline float
00141       getDistanceThreshold () const
00142       {
00143         return (distance_threshold_);
00144       }
00145 
00146       /** \brief Set label cloud
00147         * \param[in] labels The label cloud
00148         */
00149       void
00150       setLabels (PointCloudLPtr& labels)
00151       {
00152         labels_ = labels;
00153       }
00154 
00155       /** \brief Set labels in the label cloud to exclude.
00156         * \param exclude_labels a vector of bools corresponding to whether or not a given label should be considered
00157         */
00158       void
00159       setExcludeLabels (std::vector<bool>& exclude_labels)
00160       {
00161         exclude_labels_ = boost::make_shared<std::vector<bool> >(exclude_labels);
00162       }
00163 
00164       /** \brief Compare points at two indices by their plane equations.  True if the angle between the normals is less than the angular threshold,
00165         * and the difference between the d component of the normals is less than distance threshold, else false
00166         * \param idx1 The first index for the comparison
00167         * \param idx2 The second index for the comparison
00168         */
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 dist_threshold = distance_threshold_;
00182         if (depth_dependent_)
00183         {
00184           Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();
00185           float z = vec.dot (z_axis_);
00186           dist_threshold *= z * z;
00187         }
00188 
00189         float dx = input_->points[idx1].x - input_->points[idx2].x;
00190         float dy = input_->points[idx1].y - input_->points[idx2].y;
00191         float dz = input_->points[idx1].z - input_->points[idx2].z;
00192         float dist = sqrtf (dx*dx + dy*dy + dz*dz);
00193 
00194         return (dist < dist_threshold);
00195       }
00196       
00197     protected:
00198       PointCloudNConstPtr normals_;
00199       PointCloudLPtr labels_;
00200 
00201       boost::shared_ptr<std::vector<bool> > exclude_labels_;
00202       float angular_threshold_;
00203       float distance_threshold_;
00204       bool depth_dependent_;
00205       Eigen::Vector3f z_axis_;
00206   };
00207 }
00208 
00209 #endif // PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_