Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/segmentation/include/pcl/segmentation/plane_coefficient_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  * $Id$
00037  *
00038  */
00039 
00040 #ifndef PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_
00041 #define PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_
00042 
00043 #include <pcl/common/angles.h>
00044 #include <pcl/segmentation/boost.h>
00045 #include <pcl/segmentation/comparator.h>
00046 
00047 namespace pcl
00048 {
00049   /** \brief PlaneCoefficientComparator is a Comparator that operates on plane coefficients, for use in planar segmentation.
00050     * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data.
00051     *
00052     * \author Alex Trevor
00053     */
00054   template<typename PointT, typename PointNT>
00055   class PlaneCoefficientComparator: public Comparator<PointT>
00056   {
00057     public:
00058       typedef typename Comparator<PointT>::PointCloud PointCloud;
00059       typedef typename Comparator<PointT>::PointCloudConstPtr PointCloudConstPtr;
00060       
00061       typedef typename pcl::PointCloud<PointNT> PointCloudN;
00062       typedef typename PointCloudN::Ptr PointCloudNPtr;
00063       typedef typename PointCloudN::ConstPtr PointCloudNConstPtr;
00064       
00065       typedef boost::shared_ptr<PlaneCoefficientComparator<PointT, PointNT> > Ptr;
00066       typedef boost::shared_ptr<const PlaneCoefficientComparator<PointT, PointNT> > ConstPtr;
00067 
00068       using pcl::Comparator<PointT>::input_;
00069       
00070       /** \brief Empty constructor for PlaneCoefficientComparator. */
00071       PlaneCoefficientComparator ()
00072         : normals_ ()
00073         , plane_coeff_d_ ()
00074         , angular_threshold_ (pcl::deg2rad (2.0f))
00075         , distance_threshold_ (0.02f)
00076         , depth_dependent_ (true)
00077         , z_axis_ (Eigen::Vector3f (0.0, 0.0, 1.0) )
00078       {
00079       }
00080 
00081       /** \brief Constructor for PlaneCoefficientComparator.
00082         * \param[in] plane_coeff_d a reference to a vector of d coefficients of plane equations.  Must be the same size as the input cloud and input normals.  a, b, and c coefficients are in the input normals.
00083         */
00084       PlaneCoefficientComparator (boost::shared_ptr<std::vector<float> >& plane_coeff_d) 
00085         : normals_ ()
00086         , plane_coeff_d_ (plane_coeff_d)
00087         , angular_threshold_ (pcl::deg2rad (2.0f))
00088         , distance_threshold_ (0.02f)
00089         , depth_dependent_ (true)
00090         , z_axis_ (Eigen::Vector3f (0.0f, 0.0f, 1.0f) )
00091       {
00092       }
00093       
00094       /** \brief Destructor for PlaneCoefficientComparator. */
00095       virtual
00096       ~PlaneCoefficientComparator ()
00097       {
00098       }
00099 
00100       virtual void 
00101       setInputCloud (const PointCloudConstPtr& cloud)
00102       {
00103         input_ = cloud;
00104       }
00105       
00106       /** \brief Provide a pointer to the input normals.
00107         * \param[in] normals the input normal cloud
00108         */
00109       inline void
00110       setInputNormals (const PointCloudNConstPtr &normals)
00111       {
00112         normals_ = normals;
00113       }
00114 
00115       /** \brief Get the input normals. */
00116       inline PointCloudNConstPtr
00117       getInputNormals () const
00118       {
00119         return (normals_);
00120       }
00121 
00122       /** \brief Provide a pointer to a vector of the d-coefficient of the planes' hessian normal form.  a, b, and c are provided by the normal cloud.
00123         * \param[in] plane_coeff_d a pointer to the plane coefficients.
00124         */
00125       void
00126       setPlaneCoeffD (boost::shared_ptr<std::vector<float> >& plane_coeff_d)
00127       {
00128         plane_coeff_d_ = plane_coeff_d;
00129       }
00130 
00131       /** \brief Provide a pointer to a vector of the d-coefficient of the planes' hessian normal form.  a, b, and c are provided by the normal cloud.
00132         * \param[in] plane_coeff_d a pointer to the plane coefficients.
00133         */
00134       void
00135       setPlaneCoeffD (std::vector<float>& plane_coeff_d)
00136       {
00137         plane_coeff_d_ = boost::make_shared<std::vector<float> >(plane_coeff_d);
00138       }
00139       
00140       /** \brief Get a pointer to the vector of the d-coefficient of the planes' hessian normal form. */
00141       const std::vector<float>&
00142       getPlaneCoeffD () const
00143       {
00144         return (plane_coeff_d_);
00145       }
00146 
00147       /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
00148         * \param[in] angular_threshold the tolerance in radians
00149         */
00150       virtual void
00151       setAngularThreshold (float angular_threshold)
00152       {
00153         angular_threshold_ = cosf (angular_threshold);
00154       }
00155       
00156       /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
00157       inline float
00158       getAngularThreshold () const
00159       {
00160         return (acosf (angular_threshold_) );
00161       }
00162 
00163       /** \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.
00164         * \param[in] distance_threshold the tolerance in meters (at 1m)
00165         * \param[in] depth_dependent whether to scale the threshold based on range from the sensor (default: false)
00166         */
00167       void
00168       setDistanceThreshold (float distance_threshold, 
00169                             bool depth_dependent = false)
00170       {
00171         distance_threshold_ = distance_threshold;
00172         depth_dependent_ = depth_dependent;
00173       }
00174 
00175       /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
00176       inline float
00177       getDistanceThreshold () const
00178       {
00179         return (distance_threshold_);
00180       }
00181       
00182       /** \brief Compare points at two indices by their plane equations.  True if the angle between the normals is less than the angular threshold,
00183         * and the difference between the d component of the normals is less than distance threshold, else false
00184         * \param idx1 The first index for the comparison
00185         * \param idx2 The second index for the comparison
00186         */
00187       virtual bool
00188       compare (int idx1, int idx2) const
00189       {
00190         float threshold = distance_threshold_;
00191         if (depth_dependent_)
00192         {
00193           Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();
00194           
00195           float z = vec.dot (z_axis_);
00196           threshold *= z * z;
00197         }
00198         return ( (fabs ((*plane_coeff_d_)[idx1] - (*plane_coeff_d_)[idx2]) < threshold)
00199                  && (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ) );
00200       }
00201       
00202     protected:
00203       PointCloudNConstPtr normals_;
00204       boost::shared_ptr<std::vector<float> > plane_coeff_d_;
00205       float angular_threshold_;
00206       float distance_threshold_;
00207       bool depth_dependent_;
00208       Eigen::Vector3f z_axis_;
00209 
00210     public:
00211       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00212   };
00213 }
00214 
00215 #endif // PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_