Point Cloud Library (PCL)  1.7.0
plane_refinement_comparator.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id: extract_clusters.h 5027 2012-03-12 03:10:45Z rusu $
37  *
38  */
39 
40 #ifndef PCL_SEGMENTATION_PLANAR_REFINEMENT_COMPARATOR_H_
41 #define PCL_SEGMENTATION_PLANAR_REFINEMENT_COMPARATOR_H_
42 
43 #include <pcl/segmentation/boost.h>
44 #include <pcl/segmentation/plane_coefficient_comparator.h>
45 
46 namespace pcl
47 {
48  /** \brief PlaneRefinementComparator is a Comparator that operates on plane coefficients,
49  * for use in planar segmentation.
50  * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data.
51  *
52  * \author Alex Trevor, Suat Gedikli
53  */
54  template<typename PointT, typename PointNT, typename PointLT>
56  {
57  public:
60 
62  typedef typename PointCloudN::Ptr PointCloudNPtr;
64 
66  typedef typename PointCloudL::Ptr PointCloudLPtr;
68 
69  typedef boost::shared_ptr<PlaneRefinementComparator<PointT, PointNT, PointLT> > Ptr;
70  typedef boost::shared_ptr<const PlaneRefinementComparator<PointT, PointNT, PointLT> > ConstPtr;
71 
76 
77 
78  /** \brief Empty constructor for PlaneCoefficientComparator. */
80  : models_ ()
81  , labels_ ()
82  , refine_labels_ ()
83  , label_to_model_ ()
84  , depth_dependent_ (false)
85  {
86  }
87 
88  /** \brief Empty constructor for PlaneCoefficientComparator.
89  * \param[in] models
90  * \param[in] refine_labels
91  */
92  PlaneRefinementComparator (boost::shared_ptr<std::vector<pcl::ModelCoefficients> >& models,
93  boost::shared_ptr<std::vector<bool> >& refine_labels)
94  : models_ (models)
95  , labels_ ()
96  , refine_labels_ (refine_labels)
97  , label_to_model_ ()
98  , depth_dependent_ (false)
99  {
100  }
101 
102  /** \brief Destructor for PlaneCoefficientComparator. */
103  virtual
105  {
106  }
107 
108  /** \brief Set the vector of model coefficients to which we will compare.
109  * \param[in] models a vector of model coefficients produced by the initial segmentation step.
110  */
111  void
112  setModelCoefficients (boost::shared_ptr<std::vector<pcl::ModelCoefficients> >& models)
113  {
114  models_ = models;
115  }
116 
117  /** \brief Set the vector of model coefficients to which we will compare.
118  * \param[in] models a vector of model coefficients produced by the initial segmentation step.
119  */
120  void
121  setModelCoefficients (std::vector<pcl::ModelCoefficients>& models)
122  {
123  models_ = boost::make_shared<std::vector<pcl::ModelCoefficients> >(models);
124  }
125 
126  /** \brief Set which labels should be refined. This is a vector of bools 0-max_label, true if the label should be refined.
127  * \param[in] refine_labels A vector of bools 0-max_label, true if the label should be refined.
128  */
129  void
130  setRefineLabels (boost::shared_ptr<std::vector<bool> >& refine_labels)
131  {
132  refine_labels_ = refine_labels;
133  }
134 
135  /** \brief Set which labels should be refined. This is a vector of bools 0-max_label, true if the label should be refined.
136  * \param[in] refine_labels A vector of bools 0-max_label, true if the label should be refined.
137  */
138  void
139  setRefineLabels (std::vector<bool>& refine_labels)
140  {
141  refine_labels_ = boost::make_shared<std::vector<bool> >(refine_labels);
142  }
143 
144  /** \brief A mapping from label to index in the vector of models, allowing the model coefficients of a label to be accessed.
145  * \param[in] label_to_model A vector of size max_label, with the index of each corresponding model in models
146  */
147  inline void
148  setLabelToModel (boost::shared_ptr<std::vector<int> >& label_to_model)
149  {
150  label_to_model_ = label_to_model;
151  }
152 
153  /** \brief A mapping from label to index in the vector of models, allowing the model coefficients of a label to be accessed.
154  * \param[in] label_to_model A vector of size max_label, with the index of each corresponding model in models
155  */
156  inline void
157  setLabelToModel (std::vector<int>& label_to_model)
158  {
159  label_to_model_ = boost::make_shared<std::vector<int> >(label_to_model);
160  }
161 
162  /** \brief Get the vector of model coefficients to which we will compare. */
163  inline boost::shared_ptr<std::vector<pcl::ModelCoefficients> >
165  {
166  return (models_);
167  }
168 
169  /** \brief ...
170  * \param[in] labels
171  */
172  inline void
174  {
175  labels_ = labels;
176  }
177 
178  /** \brief Compare two neighboring points
179  * \param[in] idx1 The index of the first point.
180  * \param[in] idx2 The index of the second point.
181  */
182  virtual bool
183  compare (int idx1, int idx2) const
184  {
185  int current_label = labels_->points[idx1].label;
186  int next_label = labels_->points[idx2].label;
187 
188  if (!((*refine_labels_)[current_label] && !(*refine_labels_)[next_label]))
189  return (false);
190 
191  const pcl::ModelCoefficients& model_coeff = (*models_)[(*label_to_model_)[current_label]];
192 
193  PointT pt = input_->points[idx2];
194  double ptp_dist = fabs (model_coeff.values[0] * pt.x +
195  model_coeff.values[1] * pt.y +
196  model_coeff.values[2] * pt.z +
197  model_coeff.values[3]);
198 
199  // depth dependent
200  float threshold = distance_threshold_;
201  if (depth_dependent_)
202  {
203  //Eigen::Vector4f origin = input_->sensor_origin_;
204  Eigen::Vector3f vec = input_->points[idx1].getVector3fMap ();// - origin.head<3> ();
205 
206  float z = vec.dot (z_axis_);
207  threshold *= z * z;
208  }
209 
210  return (ptp_dist < threshold);
211  }
212 
213  protected:
214  boost::shared_ptr<std::vector<pcl::ModelCoefficients> > models_;
216  boost::shared_ptr<std::vector<bool> > refine_labels_;
217  boost::shared_ptr<std::vector<int> > label_to_model_;
220  };
221 }
222 
223 #endif // PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_