Point Cloud Library (PCL)  1.7.1
rgb_plane_coefficient_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  *
37  *
38  */
39 
40 #ifndef PCL_RGB_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_
41 #define PCL_RGB_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_
42 
43 #include <pcl/segmentation/boost.h>
44 #include <pcl/segmentation/plane_coefficient_comparator.h>
45 
46 namespace pcl
47 {
48  /** \brief RGBPlaneCoefficientComparator is a Comparator that operates on plane coefficients,
49  * for use in planar segmentation. Also takes into account RGB, so we can segmented different colored co-planar regions.
50  * In conjunction with OrganizedConnectedComponentSegmentation, this allows planes to be segmented from organized data.
51  *
52  * \author Alex Trevor
53  */
54  template<typename PointT, typename PointNT>
56  {
57  public:
60 
62  typedef typename PointCloudN::Ptr PointCloudNPtr;
64 
65  typedef boost::shared_ptr<RGBPlaneCoefficientComparator<PointT, PointNT> > Ptr;
66  typedef boost::shared_ptr<const RGBPlaneCoefficientComparator<PointT, PointNT> > ConstPtr;
67 
72 
73  /** \brief Empty constructor for RGBPlaneCoefficientComparator. */
75  : color_threshold_ (50.0f)
76  {
77  }
78 
79  /** \brief Constructor for RGBPlaneCoefficientComparator.
80  * \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.
81  */
82  RGBPlaneCoefficientComparator (boost::shared_ptr<std::vector<float> >& plane_coeff_d)
83  : PlaneCoefficientComparator<PointT, PointNT> (plane_coeff_d), color_threshold_ (50.0f)
84  {
85  }
86 
87  /** \brief Destructor for RGBPlaneCoefficientComparator. */
88  virtual
90  {
91  }
92 
93  /** \brief Set the tolerance in color space between neighboring points, to be considered part of the same plane.
94  * \param[in] color_threshold The distance in color space
95  */
96  inline void
97  setColorThreshold (float color_threshold)
98  {
99  color_threshold_ = color_threshold * color_threshold;
100  }
101 
102  /** \brief Get the color threshold between neighboring points, to be considered part of the same plane. */
103  inline float
105  {
106  return (color_threshold_);
107  }
108 
109  /** \brief Compare two neighboring points, by using normal information, euclidean distance, and color information.
110  * \param[in] idx1 The index of the first point.
111  * \param[in] idx2 The index of the second point.
112  */
113  bool
114  compare (int idx1, int idx2) const
115  {
116  float dx = input_->points[idx1].x - input_->points[idx2].x;
117  float dy = input_->points[idx1].y - input_->points[idx2].y;
118  float dz = input_->points[idx1].z - input_->points[idx2].z;
119  float dist = sqrtf (dx*dx + dy*dy + dz*dz);
120  int dr = input_->points[idx1].r - input_->points[idx2].r;
121  int dg = input_->points[idx1].g - input_->points[idx2].g;
122  int db = input_->points[idx1].b - input_->points[idx2].b;
123  //Note: This is not the best metric for color comparisons, we should probably use HSV space.
124  float color_dist = static_cast<float> (dr*dr + dg*dg + db*db);
125  return ( (dist < distance_threshold_)
126  && (normals_->points[idx1].getNormalVector3fMap ().dot (normals_->points[idx2].getNormalVector3fMap () ) > angular_threshold_ )
127  && (color_dist < color_threshold_));
128  }
129 
130  protected:
132  };
133 }
134 
135 #endif // PCL_SEGMENTATION_PLANE_COEFFICIENT_COMPARATOR_H_
PointCloudConstPtr input_
Definition: comparator.h:99
virtual ~RGBPlaneCoefficientComparator()
Destructor for RGBPlaneCoefficientComparator.
bool compare(int idx1, int idx2) const
Compare two neighboring points, by using normal information, euclidean distance, and color informatio...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
boost::shared_ptr< RGBPlaneCoefficientComparator< PointT, PointNT > > Ptr
boost::shared_ptr< const RGBPlaneCoefficientComparator< PointT, PointNT > > ConstPtr
Comparator< PointT >::PointCloudConstPtr PointCloudConstPtr
Comparator is the base class for comparators that compare two points given some function.
Definition: comparator.h:53
RGBPlaneCoefficientComparator(boost::shared_ptr< std::vector< float > > &plane_coeff_d)
Constructor for RGBPlaneCoefficientComparator.
RGBPlaneCoefficientComparator is a Comparator that operates on plane coefficients, for use in planar segmentation.
PointCloud::ConstPtr PointCloudConstPtr
Definition: comparator.h:58
void setColorThreshold(float color_threshold)
Set the tolerance in color space between neighboring points, to be considered part of the same plane...
PlaneCoefficientComparator is a Comparator that operates on plane coefficients, for use in planar seg...
A point structure representing Euclidean xyz coordinates, and the RGB color.
float getColorThreshold() const
Get the color threshold between neighboring points, to be considered part of the same plane...
RGBPlaneCoefficientComparator()
Empty constructor for RGBPlaneCoefficientComparator.