Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/segmentation/include/pcl/segmentation/ground_plane_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_GROUND_PLANE_COMPARATOR_H_
00041 #define PCL_SEGMENTATION_GROUND_PLANE_COMPARATOR_H_
00042 
00043 #include <pcl/common/angles.h>
00044 #include <pcl/segmentation/comparator.h>
00045 #include <boost/make_shared.hpp>
00046 
00047 namespace pcl
00048 {
00049   /** \brief GroundPlaneComparator is a Comparator for detecting smooth surfaces suitable for driving.
00050     * In conjunction with OrganizedConnectedComponentSegmentation, this allows smooth groundplanes / road surfaces to be segmented from point clouds.
00051     *
00052     * \author Alex Trevor
00053     */
00054   template<typename PointT, typename PointNT>
00055   class GroundPlaneComparator: 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<GroundPlaneComparator<PointT, PointNT> > Ptr;
00066       typedef boost::shared_ptr<const GroundPlaneComparator<PointT, PointNT> > ConstPtr;
00067 
00068       using pcl::Comparator<PointT>::input_;
00069       
00070       /** \brief Empty constructor for GroundPlaneComparator. */
00071       GroundPlaneComparator ()
00072         : normals_ ()
00073         , plane_coeff_d_ ()
00074         , angular_threshold_ (cosf (pcl::deg2rad (2.0f)))
00075         , road_angular_threshold_ ( cosf(pcl::deg2rad (10.0f)))
00076         , distance_threshold_ (0.1f)
00077         , depth_dependent_ (true)
00078         , z_axis_ (Eigen::Vector3f (0.0, 0.0, 1.0) )
00079         , desired_road_axis_ (Eigen::Vector3f(0.0, -1.0, 0.0))
00080       {
00081       }
00082 
00083       /** \brief Constructor for GroundPlaneComparator.
00084         * \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.
00085         */
00086       GroundPlaneComparator (boost::shared_ptr<std::vector<float> >& plane_coeff_d) 
00087         : normals_ ()
00088         , plane_coeff_d_ (plane_coeff_d)
00089         , angular_threshold_ (cosf (pcl::deg2rad (3.0f)))
00090         , distance_threshold_ (0.1f)
00091         , depth_dependent_ (true)
00092         , z_axis_ (Eigen::Vector3f (0.0f, 0.0f, 1.0f))
00093         , road_angular_threshold_ ( cosf(pcl::deg2rad (40.0f)))
00094         , desired_road_axis_ (Eigen::Vector3f(0.0, -1.0, 0.0))
00095       {
00096       }
00097       
00098       /** \brief Destructor for GroundPlaneComparator. */
00099       virtual
00100       ~GroundPlaneComparator ()
00101       {
00102       }
00103       /** \brief Provide the input cloud.
00104         * \param[in] cloud the input point cloud.
00105         */
00106       virtual void 
00107       setInputCloud (const PointCloudConstPtr& cloud)
00108       {
00109         input_ = cloud;
00110       }
00111       
00112       /** \brief Provide a pointer to the input normals.
00113         * \param[in] normals the input normal cloud.
00114         */
00115       inline void
00116       setInputNormals (const PointCloudNConstPtr &normals)
00117       {
00118         normals_ = normals;
00119       }
00120 
00121       /** \brief Get the input normals. */
00122       inline PointCloudNConstPtr
00123       getInputNormals () const
00124       {
00125         return (normals_);
00126       }
00127 
00128       /** \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.
00129         * \param[in] plane_coeff_d a pointer to the plane coefficients.
00130         */
00131       void
00132       setPlaneCoeffD (boost::shared_ptr<std::vector<float> >& plane_coeff_d)
00133       {
00134         plane_coeff_d_ = plane_coeff_d;
00135       }
00136 
00137       /** \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.
00138         * \param[in] plane_coeff_d a pointer to the plane coefficients.
00139         */
00140       void
00141       setPlaneCoeffD (std::vector<float>& plane_coeff_d)
00142       {
00143         plane_coeff_d_ = boost::make_shared<std::vector<float> >(plane_coeff_d);
00144       }
00145       
00146       /** \brief Get a pointer to the vector of the d-coefficient of the planes' hessian normal form. */
00147       const std::vector<float>&
00148       getPlaneCoeffD () const
00149       {
00150         return (plane_coeff_d_);
00151       }
00152 
00153       /** \brief Set the tolerance in radians for difference in normal direction between neighboring points, to be considered part of the same plane.
00154         * \param[in] angular_threshold the tolerance in radians
00155         */
00156       virtual void
00157       setAngularThreshold (float angular_threshold)
00158       {
00159         angular_threshold_ = cosf (angular_threshold);
00160       }
00161 
00162       /** \brief Set the tolerance in radians for difference in normal direction between a point and the expected ground normal.
00163         * \param[in] angular_threshold the
00164         */
00165       virtual void
00166       setGroundAngularThreshold (float angular_threshold)
00167       {
00168         road_angular_threshold_ = cosf (angular_threshold);
00169       }
00170 
00171       /** \brief Set the expected ground plane normal with respect to the sensor.  Pixels labeled as ground must be within ground_angular_threshold radians of this normal to be labeled as ground.
00172         * \param[in] normal The normal direction of the expected ground plane.
00173         */
00174       void
00175       setExpectedGroundNormal (Eigen::Vector3f normal)
00176       {
00177         desired_road_axis_ = normal;
00178       }
00179   
00180       
00181       /** \brief Get the angular threshold in radians for difference in normal direction between neighboring points, to be considered part of the same plane. */
00182       inline float
00183       getAngularThreshold () const
00184       {
00185         return (acosf (angular_threshold_) );
00186       }
00187 
00188       /** \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.
00189         * \param[in] distance_threshold the tolerance in meters (at 1m)
00190         * \param[in] depth_dependent whether to scale the threshold based on range from the sensor (default: false)
00191         */
00192       void
00193       setDistanceThreshold (float distance_threshold, 
00194                             bool depth_dependent = false)
00195       {
00196         distance_threshold_ = distance_threshold;
00197         depth_dependent_ = depth_dependent;
00198       }
00199 
00200       /** \brief Get the distance threshold in meters (d component of plane equation) between neighboring points, to be considered part of the same plane. */
00201       inline float
00202       getDistanceThreshold () const
00203       {
00204         return distance_threshold_;
00205       }
00206       
00207       /** \brief Compare points at two indices by their plane equations.  True if the angle between the normals is less than the angular threshold,
00208         * and the difference between the d component of the normals is less than distance threshold, else false
00209         * \param idx1 The first index for the comparison
00210         * \param idx2 The second index for the comparison
00211         */
00212       virtual bool
00213       compare (int idx1, int idx2) const
00214       {
00215         // Normal must be similar to neighbor
00216         // Normal must be similar to expected normal
00217         float threshold = distance_threshold_;
00218         if (depth_dependent_)
00219         {
00220           Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();
00221           
00222           float z = vec.dot (z_axis_);
00223           threshold *= z * z;
00224         }
00225 
00226         return ( (normals_->points[idx1].getNormalVector3fMap ().dot (desired_road_axis_) > road_angular_threshold_) &&
00227                  (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ));
00228         
00229         // Euclidean proximity of neighbors does not seem to be required -- pixel adjacency handles this well enough 
00230         //return ( (normals_->points[idx1].getNormalVector3fMap ().dot (desired_road_axis_) > road_angular_threshold_) &&
00231         //          (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ ) &&
00232         //         (pcl::euclideanDistance (input_->points[idx1], input_->points[idx2]) < distance_threshold_ ));
00233       }
00234       
00235     protected:
00236       PointCloudNConstPtr normals_;
00237       boost::shared_ptr<std::vector<float> > plane_coeff_d_;
00238       float angular_threshold_;
00239       float road_angular_threshold_;
00240       float distance_threshold_;
00241       bool depth_dependent_;
00242       Eigen::Vector3f z_axis_;
00243       Eigen::Vector3f desired_road_axis_;
00244 
00245     public:
00246       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00247   };
00248 }
00249 
00250 #endif // PCL_SEGMENTATION_GROUND_PLANE_COMPARATOR_H_