Point Cloud Library (PCL)
1.7.0
|
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: 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/boost.h> 00044 #include <pcl/segmentation/plane_coefficient_comparator.h> 00045 00046 namespace pcl 00047 { 00048 /** \brief PlaneRefinementComparator is a Comparator that operates on plane coefficients, 00049 * for use in planar segmentation. 00050 * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data. 00051 * 00052 * \author Alex Trevor, Suat Gedikli 00053 */ 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 00078 /** \brief Empty constructor for PlaneCoefficientComparator. */ 00079 PlaneRefinementComparator () 00080 : models_ () 00081 , labels_ () 00082 , refine_labels_ () 00083 , label_to_model_ () 00084 , depth_dependent_ (false) 00085 { 00086 } 00087 00088 /** \brief Empty constructor for PlaneCoefficientComparator. 00089 * \param[in] models 00090 * \param[in] refine_labels 00091 */ 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 00102 /** \brief Destructor for PlaneCoefficientComparator. */ 00103 virtual 00104 ~PlaneRefinementComparator () 00105 { 00106 } 00107 00108 /** \brief Set the vector of model coefficients to which we will compare. 00109 * \param[in] models a vector of model coefficients produced by the initial segmentation step. 00110 */ 00111 void 00112 setModelCoefficients (boost::shared_ptr<std::vector<pcl::ModelCoefficients> >& models) 00113 { 00114 models_ = models; 00115 } 00116 00117 /** \brief Set the vector of model coefficients to which we will compare. 00118 * \param[in] models a vector of model coefficients produced by the initial segmentation step. 00119 */ 00120 void 00121 setModelCoefficients (std::vector<pcl::ModelCoefficients>& models) 00122 { 00123 models_ = boost::make_shared<std::vector<pcl::ModelCoefficients> >(models); 00124 } 00125 00126 /** \brief Set which labels should be refined. This is a vector of bools 0-max_label, true if the label should be refined. 00127 * \param[in] refine_labels A vector of bools 0-max_label, true if the label should be refined. 00128 */ 00129 void 00130 setRefineLabels (boost::shared_ptr<std::vector<bool> >& refine_labels) 00131 { 00132 refine_labels_ = refine_labels; 00133 } 00134 00135 /** \brief Set which labels should be refined. This is a vector of bools 0-max_label, true if the label should be refined. 00136 * \param[in] refine_labels A vector of bools 0-max_label, true if the label should be refined. 00137 */ 00138 void 00139 setRefineLabels (std::vector<bool>& refine_labels) 00140 { 00141 refine_labels_ = boost::make_shared<std::vector<bool> >(refine_labels); 00142 } 00143 00144 /** \brief A mapping from label to index in the vector of models, allowing the model coefficients of a label to be accessed. 00145 * \param[in] label_to_model A vector of size max_label, with the index of each corresponding model in models 00146 */ 00147 inline void 00148 setLabelToModel (boost::shared_ptr<std::vector<int> >& label_to_model) 00149 { 00150 label_to_model_ = label_to_model; 00151 } 00152 00153 /** \brief A mapping from label to index in the vector of models, allowing the model coefficients of a label to be accessed. 00154 * \param[in] label_to_model A vector of size max_label, with the index of each corresponding model in models 00155 */ 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 00162 /** \brief Get the vector of model coefficients to which we will compare. */ 00163 inline boost::shared_ptr<std::vector<pcl::ModelCoefficients> > 00164 getModelCoefficients () const 00165 { 00166 return (models_); 00167 } 00168 00169 /** \brief ... 00170 * \param[in] labels 00171 */ 00172 inline void 00173 setLabels (PointCloudLPtr& labels) 00174 { 00175 labels_ = labels; 00176 } 00177 00178 /** \brief Compare two neighboring points 00179 * \param[in] idx1 The index of the first point. 00180 * \param[in] idx2 The index of the second point. 00181 */ 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_