plane_refinement_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  * $Id: extract_clusters.h 5027 2012-03-12 03:10:45Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_SEGMENTATION_PLANAR_REFINEMENT_COMPARATOR_H_
00041 #define PCL_SEGMENTATION_PLANAR_REFINEMENT_COMPARATOR_H_
00042 
00043 #include <pcl/segmentation/plane_coefficient_comparator.h>
00044 #include <boost/make_shared.hpp>
00045 
00046 namespace pcl
00047 {
00054   template<typename PointT, typename PointNT, typename PointLT>
00055   class PlaneRefinementComparator: public PlaneCoefficientComparator<PointT, PointNT>
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 typename pcl::PointCloud<PointLT> PointCloudL;
00066       typedef typename PointCloudL::Ptr PointCloudLPtr;
00067       typedef typename PointCloudL::ConstPtr PointCloudLConstPtr;
00068 
00069       typedef boost::shared_ptr<PlaneRefinementComparator<PointT, PointNT, PointLT> > Ptr;
00070       typedef boost::shared_ptr<const PlaneRefinementComparator<PointT, PointNT, PointLT> > ConstPtr;
00071 
00072       using pcl::PlaneCoefficientComparator<PointT, PointNT>::input_;
00073       using pcl::PlaneCoefficientComparator<PointT, PointNT>::normals_;
00074       using pcl::PlaneCoefficientComparator<PointT, PointNT>::distance_threshold_;
00075       using pcl::PlaneCoefficientComparator<PointT, PointNT>::plane_coeff_d_;
00076 
00077 
00079      PlaneRefinementComparator ()
00080         : models_ ()
00081         , labels_ ()
00082         , refine_labels_ ()
00083         , label_to_model_ ()
00084         , depth_dependent_ (false)
00085       {
00086       }
00087 
00092       PlaneRefinementComparator (boost::shared_ptr<std::vector<pcl::ModelCoefficients> >& models,
00093                                  boost::shared_ptr<std::vector<bool> >& refine_labels)
00094         : models_ (models)
00095         , labels_ ()
00096         , refine_labels_ (refine_labels)
00097         , label_to_model_ ()
00098         , depth_dependent_ (false)
00099       {
00100       }
00101 
00103       virtual
00104       ~PlaneRefinementComparator ()
00105       {
00106       }
00107 
00111       void
00112       setModelCoefficients (boost::shared_ptr<std::vector<pcl::ModelCoefficients> >& models)
00113       {
00114         models_ = models;
00115       }
00116 
00120       void
00121       setModelCoefficients (std::vector<pcl::ModelCoefficients>& models)
00122       {
00123         models_ = boost::make_shared<std::vector<pcl::ModelCoefficients> >(models);
00124       }
00125 
00129       void
00130       setRefineLabels (boost::shared_ptr<std::vector<bool> >& refine_labels)
00131       {
00132         refine_labels_ = refine_labels;
00133       }
00134       
00138       void
00139       setRefineLabels (std::vector<bool>& refine_labels)
00140       {
00141         refine_labels_ = boost::make_shared<std::vector<bool> >(refine_labels);
00142       }
00143 
00147       inline void
00148       setLabelToModel (boost::shared_ptr<std::vector<int> >& label_to_model)
00149       {
00150         label_to_model_ = label_to_model;
00151       }
00152       
00156       inline void
00157       setLabelToModel (std::vector<int>& label_to_model)
00158       {
00159         label_to_model_ = boost::make_shared<std::vector<int> >(label_to_model);
00160       }
00161 
00163       inline boost::shared_ptr<std::vector<pcl::ModelCoefficients> >
00164       getModelCoefficients () const
00165       {
00166         return (models_);
00167       }
00168 
00172       inline void
00173       setLabels (PointCloudLPtr& labels)
00174       {
00175         labels_ = labels;
00176       }
00177 
00182       virtual bool
00183       compare (int idx1, int idx2) const
00184       {
00185         int current_label = labels_->points[idx1].label;
00186         int next_label = labels_->points[idx2].label;
00187 
00188         if (!((*refine_labels_)[current_label] && !(*refine_labels_)[next_label]))
00189           return (false);
00190         
00191         const pcl::ModelCoefficients& model_coeff = (*models_)[(*label_to_model_)[current_label]];
00192         
00193         PointT pt = input_->points[idx2];
00194         double ptp_dist = fabs (model_coeff.values[0] * pt.x + 
00195                                 model_coeff.values[1] * pt.y + 
00196                                 model_coeff.values[2] * pt.z +
00197                                 model_coeff.values[3]);
00198         
00199         // depth dependent
00200         float threshold = distance_threshold_;
00201         if (depth_dependent_)
00202         {
00203           //Eigen::Vector4f origin = input_->sensor_origin_;
00204           Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();// - origin.head<3> ();
00205           
00206           float z = vec.dot (z_axis_);
00207           threshold *= z * z;
00208         }
00209         
00210         return (ptp_dist < threshold);
00211       }
00212 
00213     protected:
00214       boost::shared_ptr<std::vector<pcl::ModelCoefficients> > models_;
00215       PointCloudLPtr labels_;
00216       boost::shared_ptr<std::vector<bool> > refine_labels_;
00217       boost::shared_ptr<std::vector<int> > label_to_model_;
00218       bool depth_dependent_;
00219       using PlaneCoefficientComparator<PointT, PointNT>::z_axis_;
00220   };
00221 }
00222 
00223 #endif // PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:16:32